/**
  ******************************************************************************
  * @file	  moveBase.c
  * @author	  Action
  * @version   V1.0.0
  * @date	  2019/08/01
  * @brief	 2019省赛底盘运动控制部分
  ******************************************************************************
  * @attention
  *			None
  ******************************************************************************
  */
/* Includes -------------------------------------------------------------------------------------------*/
#include "moveBase.h"
#include "includes.h"
#include <app_cfg.h>
#include "misc.h"
#include "stm32f4xx_it.h"
#include "stm32f4xx_usart.h"
#include "stm32f4xx_gpio.h"
#include "stm32f4xx_rcc.h"
#include "timer.h"
#include "gpio.h"
#include "usart.h"
#include "can.h"
#include "elmo.h"
#include "fort.h"
#include "pps.h"
#include "math.h"
#include "moveprote.h"
extern int temp9,lastMode;
extern int mode5Flag,mode5StopAgl;
extern float shooterVel,yawPos;
static int nowTime=0 ,lastTime=0;
static float timeChange=0;
float detv1=0;
int detr1=0,lastDetr1=0;
extern int x,y,speedX,speedY,angleZ;
extern float angle;
extern int timeCnt,correctFlag;
extern int vCheckL,vCheckR;
extern int32_t actual1Vel,actual2Vel,actual7Pos;
int vTime=0;
int goBackFlag=0,cycleTimes=0,crashTimes=0;
int blueBallStore=0,redBallStore=0;
extern int mode;
int squareFlag=0;
int pinkColorFlag=0;
int timeBallNothingCnt=0;
extern char ballColor[];
extern int aimActualPosFlag;
extern int motorActualPos;
extern int keyOneStatus,keyTwoStatus,keyThreeStatus,keyFourStatus,keyRedBlueStatus,keyWolkingStatus,keyCheckStatus;  
extern int usartBallNothingCnt,usartMotorActualPos,usartCrashMod,usartModFlag;
extern int colorChangeFlag;
int stopCollectRunFlag=0;
/**
* @brief 定义结构体PID
* @note	无
*/
 struct PID  
{
	float kp;														
	float ki;														
	float kd;														
	float nowErr;												
	float lastErr;											
	float errChange;										
	float dErr;												
	float errSum;										
	float outPut;											
};				

void RunShoot(void)
{
	detr1=0;
	detv1=0;
	PidBiggerRoundOri(1,1200 ,0); 
	collectBall();
}
/**
* @brief 	速度m/s转换为脉冲
*/
int ExchangeVel(float v)
{
	v=(int)(v/(0.120*PI)*32768);
	return v;
}

/**
* @brief 	给定速度直线前进 
* @param 	速度 单位m/s   前进v为正，倒退v为负
* @note		开环
*/
void GoStraight(float v)
{		
	VelCrl(CAN1, 1,-ExchangeVel(v));
	VelCrl(CAN1, 2,ExchangeVel(v));
}

/**
* @brief	 	圆形闭环从出发区走
* @param		速度 m/s 
* @param		半径 mm  
* @param		模式1顺0逆  
* @note			无
* @note   	需要调用PidRoundOri这个函数，需要将PidRoundOri这个函数参数调好，与PidRound区别开。
* @note			以（0，2400）为圆心，先直走一部分，随后拐到圆上。
*/
void OriGoPidRou(float v,float r,short modeRou)
{
	static int moveMode=0;
	static float detr=0,detv=0;
	if(modeRou==1)
	{
		if(moveMode==0)
		{
			AnyLineVel(v,1,1,1.7*r-2400,-1);	//v,1,1,1.414*r-2400,-1	
			if(x<-(0.707*r-50))//500提前量
			{
				moveMode++;
			}
		}
		else 
		{
//			PidRound(v,r,1);           //串级圆
			PidRoundOri(v,r,1);					 //并级圆  
		}
	}
	else
	{
		if(moveMode==0)
		{
			AnyLineVel(v,-1,1,2*r-2400,1);	//v,1,1,1.414*r-2400,-1	
			if(x>(0.707*r-50))//500提前量
			{
				moveMode++;
			}
		}
		else 
		{
			PidRoundOri(v+0.001*detv,r+detr,0);
		}
	}
	
	
	
//		detv+=0.6;  //随时间增加
//		detr+=0.3;
	if(r+detr>2000)
		{
			detv-=0.6;
			detr-=0.5;
		}
		
//			if(moveMode==0)
//	{
//		AnyLineVel(1.3,1,0,0,1);  //所有的速度改为了m/s的单位
//		if(y>500)          //先直走他个1200
//		{
//			moveMode=1;
//		}
//	}
//	else if(moveMode==1)
//	{
//		AnyLineVel(1.0,1,0,0,1);
//		if(y>2000-r)
//		{
//			moveMode=2;
//		}
//	}
//	else if(moveMode==2)
//	{
//		AnyLineVel(0.9,0,1,-1*(2300-r),-1);
//		if(angle>75)
//		{
//			moveMode=3;
//		}
//	}
}
/**
* @brief	 	圆形闭环(be able to start from the beginning area)
* @note			无
* @param 	 	v 速度 m/s
* @param  	r 半径 mm
* @param  	modePidRound 1顺0逆
* @note   	注意把该函数与PidRound区别开来。
*/
void PidRoundOri(float v0, float r ,int modePidRound) 
{
	static int circleFlag=0,circleTimes=0;
	float v=v0*1000;
	float v1,v2;
	float angleGoal; 
	int pulse1,pulse2;
	static struct PID PidRouDistan_t={0,0,20};  //1.3,0,50,1.8,0,50
	static struct PID PidRouAngle_t={0,0,10};
	if(x>-1&&x<1)                                        //四个特殊点的angleGoal
	{
		if(y<2325)
		{
			angleGoal=-90;
		}
		else angleGoal=90;
	}
	else if(y<2326&&y>2324)
	{
		if(x>0)
		{
			angleGoal=0; 
		}
		else angleGoal=180;
	}
	else                                           //一般情况下的angleGoal
	{
		angleGoal=atan2((y-2325)*1.0,x*1.0)*180/PI;
	}
	if(modePidRound==1)												//顺时针走圆的时候，angleGoal与逆时针的有一定关系
	{
		if(y<2315)
		{
			angleGoal=angleGoal+180;
		}
		else angleGoal=angleGoal-180;
	}
	if(angleGoal>0&&angle<angleGoal-180)			//保证走劣弧
	{
		angle=angle+360;
	}
	else if(angleGoal<0&&angle>angleGoal+180)
	{            
		angle=angle-360;
	}
	/*PID参数调节*/
	PidRouDistan_t.nowErr=r-sqrt(x*x+(y-2325)*(y-2325));
	PidRouDistan_t.dErr=PidRouDistan_t.nowErr-PidRouDistan_t.lastErr;
//	PidRouDistan_t.kp=(900-170.0/r*v)/(2400-r)+0.05+0.9*(1-fabs(PidRouDistan_t.nowErr)/(2400-r));
	PidRouDistan_t.kp=(900-170.0/r*v)/(2325-r)+1.8;
	if(modePidRound==1)
	{		
		PidRouAngle_t.nowErr=angleGoal-angle;					//PID参数
	}
	else
	{
		PidRouAngle_t.nowErr=angleGoal-angle;
	}
	if(fabs(PidRouDistan_t.nowErr)>500)
	{
		PidRouAngle_t.kp=45;
	}
	else
	{
		PidRouAngle_t.kp=25 ;
	}
	PidRouDistan_t.outPut=PidRouDistan_t.kp*PidRouDistan_t.nowErr+PidRouDistan_t.kd*PidRouDistan_t.dErr;
	PidRouAngle_t.dErr=PidRouAngle_t.nowErr-PidRouAngle_t.lastErr;
	PidRouAngle_t.outPut=PidRouAngle_t.kp*PidRouAngle_t.nowErr+PidRouAngle_t.kd*PidRouAngle_t.dErr;
	if(modePidRound==0)																				//限幅
	{		
		if(PidRouDistan_t.outPut-PidRouAngle_t.outPut>800)
		{
			PidRouDistan_t.outPut=800;
			PidRouAngle_t.outPut=0;
		}
		if(PidRouDistan_t.outPut-PidRouAngle_t.outPut<-800)
		{
			PidRouDistan_t.outPut=-800;
			PidRouAngle_t.outPut=0;
		}
		v1=(r-177.7)/r*v+PidRouDistan_t.outPut-PidRouAngle_t.outPut;
		v2=(r+177.7)/r*v-PidRouDistan_t.outPut+PidRouAngle_t.outPut;
	}
	if(modePidRound==1)
	{
		if(PidRouDistan_t.outPut+PidRouAngle_t.outPut>800)
		{
			PidRouDistan_t.outPut=800;
			PidRouAngle_t.outPut=0;
		}
		if(PidRouDistan_t.outPut+PidRouAngle_t.outPut<-800)
		{
			PidRouDistan_t.outPut=-800;
			PidRouAngle_t.outPut=0;
		}
		v1=(r+177.7)/r*v-PidRouDistan_t.outPut-PidRouAngle_t.outPut; 
		v2=(r-177.7)/r*v+PidRouDistan_t.outPut+PidRouAngle_t.outPut;
	}
//	temp5=v1;
//	temp6=v2;
	pulse1=(int)(v1/(120*PI)*32768);
	pulse2=(int)(v2/(120*PI)*32768);
	VelCrl(CAN1, 1,-pulse1);
	VelCrl(CAN1, 2,pulse2);
	vCheckL=-pulse1;
	vCheckR=pulse2;
	PidRouDistan_t.lastErr=PidRouDistan_t.nowErr;
	PidRouAngle_t.lastErr=PidRouAngle_t.nowErr;
//	temp2=PidRouAngle_t.nowErr;
	if(x>10) circleFlag=1;
	if(x<0&&circleFlag==1)
	{
		circleTimes++;
		circleFlag=0;
	}
	if(circleTimes>9)
	{
		mode=8;
		detr1=0;
		detv1=0;
		circleTimes=0;
	}
}

/**	
*	@brief	角度PID调节
*	@param	单位 度
*/
void PidAngle(int setAngle)
{
	static struct PID PidAngle_t={0.02,0,0};
	PidAngle_t.nowErr=setAngle-angle;
	PidAngle_t.errSum+=PidAngle_t.nowErr;
	PidAngle_t.dErr=PidAngle_t.nowErr-PidAngle_t.lastErr;
	if(PidAngle_t.nowErr<-180)
	{
		angle=angle-360;
		PidAngle_t.nowErr=setAngle-angle;
	}
	if(PidAngle_t.nowErr>180)
	{
		angle=angle+360;
		PidAngle_t.nowErr=setAngle-angle;
	}
	PidAngle_t.outPut=PidAngle_t.kp*PidAngle_t.nowErr+PidAngle_t.ki*PidAngle_t.errSum+PidAngle_t.kd*PidAngle_t.dErr;
	PidAngle_t.lastErr=PidAngle_t.nowErr;
	float v=PidAngle_t.outPut;
	
	/*角速度限制小于200度*/
	if(v>((200/180.f)*PI*0.17))
	{
		v=(200/180.f)*PI*0.17;
	}
	if(v<-((200/180.f)*PI*0.17))
	{
		v=-(200/180.f)*PI*0.17;
	}					
	VelCrl(CAN1, 1,ExchangeVel(v));
	VelCrl(CAN1, 2,ExchangeVel(v));	
}


/**
* @brief  任意直线
* @param  速度  			单位 m/s（毫米） 
* @param  ax+by+c=0  	单位毫米
* @param  direction  	方向走直线的那一边   一四象限为1，二三象限-1,斜率不存在向上为1.
* @author ACTION		
* @note 	速度单位是米每秒请注意,但是截距是负的
*/
void AnyLineVel(float v1,float a, float b,float c,short dir)
{
	float v;
	v=v1*1000;
	double angleGoal=0,detV=0;
	float k=0,intercept=0;       //定义斜率截距
	nowTime=timeCnt;
	timeChange=(nowTime-lastTime)/1000.0;
	static struct PID distancePID_t={5,0.006,0.6},anglePID_t={5.5,0.004,0.6};	
	if(b!=0)                           //斜率存在
	{	
		k=-1*a/b;
		intercept=-1*c/b;
		if(k>0)                          //斜率>0
		{                        																														
			angleGoal=atan(k)*180/PI-90;																											
			if(dir==-1)                    //二三象限 负方向										
				angleGoal=angleGoal+180;
		}
		else if(k<0)                     //斜率<0
		{
			angleGoal=atan(k)*180/PI+90;
			if(dir==1)                     //一四象限 负方向
				angleGoal=angleGoal-180;
		}
		else                            //斜率=0
		{
			if(dir==1)										//在一四象限
			{
				angleGoal=-90;
			}
			else angleGoal=90;            //在二三象限
		}
		if(angleGoal>0&&angle<angleGoal-180)
		{
			angle=angle+360;
		}
		if(angleGoal<0&&angle>angleGoal+180)
		{            
			angle=angle-360;
		}
		if(intercept<0)                                   //截距小于0的时候，给pid值
		{
			distancePID_t.kp=6;
			distancePID_t.ki=0.002;
			anglePID_t.kp=6;
			anglePID_t.ki=0.001;
			anglePID_t.kd=0.9;
		}
		else if(intercept>0)														//截距大于0的时候
		{
			distancePID_t.kp=anglePID_t.kp*fabs(angleGoal)/(fabs(intercept)/sqrt(1+k*k)/10.0);
		}
		else                                         //截距=0
		{			
			distancePID_t.kp=6;
		}
		if(k==0)                                 //斜率等于零的时候
		{
			distancePID_t.nowErr=(y-intercept)/10;    
			anglePID_t.kp=0.024*v;                                //走方形的时候的kp值=0.02*v
			anglePID_t.kd=0.2;
			anglePID_t.ki=0;
			distancePID_t.kp=0.011*v;
			distancePID_t.ki=0.001;
			distancePID_t.kd=0.3;
		}
		else                                   //斜率不等于零的时候的nowerr
		{
			distancePID_t.nowErr=(y-k*x-intercept)/sqrt(1+k*k)/10;
		}
		distancePID_t.errChange=distancePID_t.nowErr-distancePID_t.lastErr;
		distancePID_t.dErr=distancePID_t.errChange/timeChange;
		distancePID_t.errSum+=(distancePID_t.nowErr*timeChange);
			
		anglePID_t.nowErr=angleGoal-angle;
		anglePID_t.errChange=anglePID_t.nowErr-anglePID_t.lastErr;
		anglePID_t.dErr=anglePID_t.errChange/timeChange;
		anglePID_t.errSum+=(anglePID_t.nowErr*timeChange);
	}
	else                        // 斜率不存在
	{
		if(dir==1)              //一二象限,向上
		{
			angleGoal=0;
		}
		else if(-1*c/a>0)angleGoal=-180;   // 三象限
		else angleGoal=180;                   //四象限
		
		if(angleGoal>0&&angle<angleGoal-180)  //转劣弧
		{
			angle=angle+360;
		}
		else if(angleGoal<0&&angle>angleGoal+180)
		{
			angle=angle-360;
		}
		distancePID_t.nowErr=(-1*c/a-x)/10;
		distancePID_t.errChange=distancePID_t.nowErr-distancePID_t.lastErr;
		distancePID_t.dErr=distancePID_t.errChange/timeChange;
		distancePID_t.errSum+=(distancePID_t.nowErr*timeChange);

		anglePID_t.nowErr=angleGoal-angle;
		anglePID_t.errChange=anglePID_t.nowErr-anglePID_t.lastErr;
		anglePID_t.dErr=anglePID_t.errChange/timeChange;
		anglePID_t.errSum+=(anglePID_t.nowErr*timeChange);
		distancePID_t.kp=0.011*v;                                     //斜率不存在的时候的pid参数
		distancePID_t.ki=0;
		distancePID_t.kd=0.48;
		anglePID_t.kp=0.024*v;                                         //0.02*v  与速度有关
		anglePID_t.ki=0;
		anglePID_t.kd=0.48;
	}
	distancePID_t.outPut=distancePID_t.kp*distancePID_t.nowErr+distancePID_t.ki*distancePID_t.errSum+distancePID_t.kd*distancePID_t.dErr;
	anglePID_t.outPut=anglePID_t.kp*anglePID_t.nowErr+anglePID_t.ki*anglePID_t.errSum+anglePID_t.kd*anglePID_t.dErr;
	if(fabs(distancePID_t.outPut)>fabs(anglePID_t.outPut)&&fabs(anglePID_t.outPut)>800)    //限幅
	{
		distancePID_t.outPut=0.8*distancePID_t.outPut;
	}
	
	detV=anglePID_t.outPut-dir*distancePID_t.outPut;
	if(detV>1500||detV<-1500)
	{
		anglePID_t.outPut=0.95*anglePID_t.outPut;
		anglePID_t.outPut=0.95*anglePID_t.outPut;
		detV=anglePID_t.outPut-dir*distancePID_t.outPut;
	}
	VelCrl(CAN1,1,(int)(32768*(detV-v)/(120*PI)));
	VelCrl(CAN1,2,(int)(32768*(detV+v)/(120*PI)));
	vCheckL=(int)(32768*(detV-v)/(120*PI));
	vCheckR=(int)(32768*(detV+v)/(120*PI));
	anglePID_t.kp=5.5;
	lastTime=nowTime;
	distancePID_t.lastErr=distancePID_t.nowErr;
	anglePID_t.lastErr=anglePID_t.nowErr;
}

/**
* @brief	角度闭环位置闭环正方形
* @param 	速度单位	m/s
* @param	边长单位	mm
* @parammodePidRound 1顺0逆
*/
void PidSquare(float v,float d,int modePidSquare)
{
	if(modePidSquare)
	{
//		int modeFlag;                  //该段程序用在小车没从出发去出发情况
//		static int squareFlag=0;
//		if(squareFlag==0)
//		{
//			if(y>2400)
//			{
//				modeFlag=2;
//			}
//			else
//			{
//				modeFlag=0;
//			}
//      squareFlag=1;
//		}                               //到此
		int vFlag=0;
		static int modeFlag=-1;             //起始阶段直线车从出发去靠近直线
		int setAngle;
		static struct PID distancePID_t={0.115,0,0};     //0.1  950
		if(modeFlag==-1)
		{	
			setAngle=0;
			distancePID_t.nowErr=x+1000;     //判定距离直线左边为负，右边为正
			distancePID_t.outPut=distancePID_t.kp*distancePID_t.nowErr;
			if(distancePID_t.outPut>60)
			{
				distancePID_t.outPut=60;
			}
			if(distancePID_t.outPut<-60)
			{
				distancePID_t.outPut=-60;
			}
			setAngle=setAngle+(int)distancePID_t.outPut;
			if(y>2350)
			{
				modeFlag=2;
			}
		}
		if(modeFlag==0)
		{
//				if(y>950&&y<1700&&x<1000&&x>360)
//				{
//					v=v-0.6;
//					vFlag=1;
//				}
			setAngle=90;
			distancePID_t.nowErr=y-1000;     //判定距离直线上方为正，下方为负
			distancePID_t.kp=0.1;
			distancePID_t.outPut=distancePID_t.kp*distancePID_t.nowErr;
			if(distancePID_t.outPut>100)
			{
				distancePID_t.outPut=100;
			}
			if(distancePID_t.outPut<-100)
			{
				distancePID_t.outPut=-100;
			}
			setAngle=setAngle+(int)distancePID_t.outPut;
			if(x<-340)
			{
				modeFlag=1;
			}
		}
		if(modeFlag==1)
		{
//				if(x<-300&&x>-1000&&y>950&&y<1650)
//				{
//					v=v-0.6;
//					vFlag=1;
//				}
			setAngle=0;
			distancePID_t.nowErr=x+1000;     //判定距离直线左边为负，右边为正
			distancePID_t.kp=0.1;
			distancePID_t.outPut=distancePID_t.kp*distancePID_t.nowErr;
			if(distancePID_t.outPut>100)
			{
				distancePID_t.outPut=100;
			}
			if(distancePID_t.outPut<-100)
			{
				distancePID_t.outPut=-100;
			}
			setAngle=setAngle+(int)distancePID_t.outPut;
			if(y>2320)
			{
				modeFlag=2;
			}
		}
		if(modeFlag==2)
		{
//				if(y>2300&&y<3050&&x<-360&&x>-1000)
//				{
//					v=v-0.6;
//					vFlag=1;
//				}
			setAngle=-90;
			distancePID_t.nowErr=y-3000;     	//判定距离直线上方为正，下方为负
			distancePID_t.kp=0.1;
			distancePID_t.outPut=distancePID_t.kp*distancePID_t.nowErr;
			if(distancePID_t.outPut>100)
			{
				distancePID_t.outPut=100;
			}
			if(distancePID_t.outPut<-100)
			{
				distancePID_t.outPut=-100;
			}
			setAngle=setAngle-(int)distancePID_t.outPut;
			if(x>340)
			{
				modeFlag=3;
			}
		}
		if(modeFlag==3)
		{
//				if(x<1050&&x>300&&y<3000&&y>2360)
//				{
//					v=v-0.6;
//					vFlag=1;
//				}
			setAngle=-180;
			distancePID_t.nowErr=x-1000;   		//判定距离直线左边为负，右边为正
			distancePID_t.kp=0.1;
			distancePID_t.outPut=distancePID_t.kp*distancePID_t.nowErr;
			if(distancePID_t.outPut>100)
			{
				distancePID_t.outPut=100;
			}
			if(distancePID_t.outPut<-100)
			{
				distancePID_t.outPut=-100;
			}
			setAngle=setAngle-(int)distancePID_t.outPut;
			if(y<1660)
			{
				modeFlag=0;
			}
		}
//		temp2=setAngle;
//		temp3=(int)distancePID_t.outPut;
		static struct PID PidAngle_t={0.05,0,0};     //0.025
		PidAngle_t.nowErr=setAngle-angle;
	//	PidAngle_t.errSum+=PidAngle_t.nowErr;
	//	PidAngle_t.dErr=PidAngle_t.nowErr-PidAngle_t.lastErr;
		if(PidAngle_t.nowErr<-180)
		{
			angle=angle-360;
			PidAngle_t.nowErr=setAngle-angle;
		}
		if(PidAngle_t.nowErr>180)
		{
			angle=angle+360;
			PidAngle_t.nowErr=setAngle-angle;
		}
		PidAngle_t.outPut=PidAngle_t.kp*PidAngle_t.nowErr+PidAngle_t.ki*PidAngle_t.errSum+PidAngle_t.kd*PidAngle_t.dErr;
	//	PidAngle_t.lastErr=PidAngle_t.nowErr;
		/*角速度限制小于200度*/
		if(PidAngle_t.outPut>((250/180.f)*PI*0.17))
		{
			PidAngle_t.outPut=(250/180.f)*PI*0.17;
		}
		if(PidAngle_t.outPut<-((250/180.f)*PI*0.17))
		{
			PidAngle_t.outPut=-(250/180.f)*PI*0.17;
		}					
		VelCrl(CAN1, 1,-ExchangeVel(v-PidAngle_t.outPut));
		VelCrl(CAN1, 2,ExchangeVel(v+PidAngle_t.outPut));	
//				if(vFlag==1)
//				{
//					v=v+0.6;
//					vFlag=0;
//				}
	}
	else
		{
//		int modeFlag;                  //该段程序用在小车没从出发去出发情况
//		static int squareFlag=0;
//		if(squareFlag==0)
//		{
//			if(y>2400)
//			{
//				modeFlag=2;
//			}
//			else
//			{
//				modeFlag=0;
//			}
//      squareFlag=1;
//		}                               //到此
			int vFlag=0;
			static int modeFlag=-1;             //起始阶段直线车从出发去靠近直线
			int setAngle;
			static struct PID distancePID_t={0.115,0,0};     //0.1  950
			if(modeFlag==-1)
			{
				setAngle=0;
				distancePID_t.nowErr=x-1000;     //判定距离直线左边为负，右边为正
				distancePID_t.outPut=distancePID_t.kp*distancePID_t.nowErr;
				if(distancePID_t.outPut>60)
				{
					distancePID_t.outPut=60;
				}
				if(distancePID_t.outPut<-60)
				{
					distancePID_t.outPut=-60;
				}
				setAngle=setAngle+(int)distancePID_t.outPut;
				if(y>2350)
				{
					modeFlag=2;
				}
			}
			if(modeFlag==0)
			{
//				if(y>950&&y<1700&&x>-1000&&x<-360)
//				{
//					v=v-0.6;
//					vFlag=1;
//				}
				setAngle=-90;
				distancePID_t.nowErr=y-1000;     //判定距离直线上方为正，下方为负
				distancePID_t.kp=0.1;
				distancePID_t.outPut=distancePID_t.kp*distancePID_t.nowErr;
				if(distancePID_t.outPut>100)
				{
					distancePID_t.outPut=100;
				}
				if(distancePID_t.outPut<-100)
				{
					distancePID_t.outPut=-100;
				}
				setAngle=setAngle-(int)distancePID_t.outPut;
				if(x>340)
				{
					modeFlag=1;
				}
			}
			if(modeFlag==1)
			{
//				if(x>300&&x<1000&&y>950&&y<1650)
//				{
//					v=v-0.6;
//					vFlag=1;
//				}
				setAngle=0;
				distancePID_t.nowErr=x-1000;     //判定距离直线左边为负，右边为正
				distancePID_t.kp=0.1;
				distancePID_t.outPut=distancePID_t.kp*distancePID_t.nowErr;
				if(distancePID_t.outPut>100)
				{
					distancePID_t.outPut=100;
				}
				if(distancePID_t.outPut<-100)
				{
					distancePID_t.outPut=-100;
				}
				setAngle=setAngle+(int)distancePID_t.outPut;
				if(y>2320)
				{
					modeFlag=2;
				}
			}
			if(modeFlag==2)
			{
//				if(y>2300&&y<3050&&x>360&&x<1000)
//				{
//					v=v-0.6;
//					vFlag=1;
//				}
				setAngle=90;
				distancePID_t.nowErr=y-3000;     	//判定距离直线上方为正，下方为负
				distancePID_t.kp=0.1;
				distancePID_t.outPut=distancePID_t.kp*distancePID_t.nowErr;
				if(distancePID_t.outPut>100)
				{
					distancePID_t.outPut=100;
				}
				if(distancePID_t.outPut<-100)
				{
					distancePID_t.outPut=-100;
				}
				setAngle=setAngle+(int)distancePID_t.outPut;
				if(x<-340)
				{
					modeFlag=3;
				}
			}
			if(modeFlag==3)
			{
//				if(x>-1050&&x<-300&&y<3000&&y>2360)
//				{
//					v=v-0.6;
//					vFlag=1;
//				}
				setAngle=180;
				distancePID_t.nowErr=x+1000;   		//判定距离直线左边为负，右边为正
				distancePID_t.kp=0.1;
				distancePID_t.outPut=distancePID_t.kp*distancePID_t.nowErr;
				if(distancePID_t.outPut>100)
				{
					distancePID_t.outPut=100;
				}
				if(distancePID_t.outPut<-100)
				{
					distancePID_t.outPut=-100;
				}
				setAngle=setAngle-(int)distancePID_t.outPut;
				if(y<1660)
				{
					modeFlag=0;
				}
			}
//			temp2=setAngle;
//			temp3=(int)distancePID_t.outPut;
			static struct PID PidAngle_t={0.05,0,0};     //0.025
			PidAngle_t.nowErr=setAngle-angle;
		//	PidAngle_t.errSum+=PidAngle_t.nowErr;
		//	PidAngle_t.dErr=PidAngle_t.nowErr-PidAngle_t.lastErr;
			if(PidAngle_t.nowErr<-180)
			{
				angle=angle-360;
				PidAngle_t.nowErr=setAngle-angle;
			}
			if(PidAngle_t.nowErr>180)
			{
				angle=angle+360;
				PidAngle_t.nowErr=setAngle-angle;
			}
			PidAngle_t.outPut=PidAngle_t.kp*PidAngle_t.nowErr+PidAngle_t.ki*PidAngle_t.errSum+PidAngle_t.kd*PidAngle_t.dErr;
		//	PidAngle_t.lastErr=PidAngle_t.nowErr;
			/*角速度限制小于200度*/
			if(PidAngle_t.outPut>((250/180.f)*PI*0.17))
			{
				PidAngle_t.outPut=(250/180.f)*PI*0.17;
			}
			if(PidAngle_t.outPut<-((250/180.f)*PI*0.17))
			{
				PidAngle_t.outPut=-(250/180.f)*PI*0.17;
			}					
			VelCrl(CAN1, 1,-ExchangeVel(v-PidAngle_t.outPut));
			VelCrl(CAN1, 2,ExchangeVel(v+PidAngle_t.outPut));	
//				if(vFlag==1)
//				{
//					v=v+0.6;
//					vFlag=0;
//				}
	}
}

/**
* @brief 	逃逸函数
* @param 	modeResume表示逃逸模式几
* @note 	goBackFlag为0正常运行，为一的时候运行撞后恢复程序。
*/
static int crashMod=0;
int edgeWall=-1,centerWall=-1,carCrash=-1;
void Resume()     
{
	static int lastx,lasty;  //上次相撞坐标
	float vTrue=sqrt((float)(speedX*speedX)/1000000.f+(float)(speedY*speedY)/1000000.f);
	/*
	撞车之后后转弯再前转弯绕开，第二次转弯半径大。
	*/
	if((vTrue<0.2)||goBackFlag==1)
	{
		vTime++;
		if(vTime>120)    //是否停止了1.2秒
		{
			goBackFlag=1;
		}
		if(goBackFlag==1&&vTime<300)       							//满足条件后退
		{
			if(vTime==121)
			{
				crashTimes++;                //碰撞次数+1
				if(abs(x-lastx)>100&&abs(y-lasty)>100)     //判断是新的碰撞还是碰撞二次
				{
					edgeWall=-1;
					centerWall=-1;
					carCrash=-1;
				}
				lastx=x;
				lasty=y;
				if(x>1900||x<-1900||y<500||y>4300)   				//撞在外围墙上  车身400*400  原始数据 2100,-2100,300,4500
				{
					edgeWall++;
					edgeWall=edgeWall%2;															//收数用 1 边缘 2 中心 3 车车					
					crashMod=1;
				}
				else if(x<700&&x>-700&&y<3100&&y>1700)     	//撞在中心柱子上  （550*550）/2+400*400=675*675   原始数据 500，-500，2850，1850
				{
					edgeWall++;
					edgeWall=edgeWall%2;
					crashMod=2;					
				}
				else     																	//与车相撞
				{
					carCrash++;
					carCrash=carCrash%4;
					crashMod=3;
					if(carCrash==3)
					{
						if(mode==0) mode=1;
						else if(mode==1) mode=0;
						else mode=4;
					}
				}
			}
			if(crashMod==1)
			{
				if(edgeWall==0)                              //第一次撞到边缘，后退走原来的路线，第二次则走圆
				{
					VelCrl(CAN1,1,ExchangeVel(1.0));
					VelCrl(CAN1,2,-ExchangeVel(1.0));
				}
				else
				{
					if(keyWolkingStatus==4||keyWolkingStatus==5||keyWolkingStatus==6)
					{
						VelCrl(CAN1,1,ExchangeVel(0.8));
						VelCrl(CAN1,2,-ExchangeVel(1.2));
					}
					else 
					{
						VelCrl(CAN1,1,ExchangeVel(1.2));
						VelCrl(CAN1,2,-ExchangeVel(0.8));
					}
					mode=4;
				}
			}
			else if(crashMod==2)
			{
				if(centerWall==0)                              //第一次撞到中心，后退走原来的路线，第二次则走圆(mode=4)
				{
					VelCrl(CAN1,1,ExchangeVel(1.0));
					VelCrl(CAN1,2,-ExchangeVel(1.0));
				}
				else
				{
					if(keyWolkingStatus==4||keyWolkingStatus==5||keyWolkingStatus==6)
					{
						VelCrl(CAN1,1,ExchangeVel(0.9));
						VelCrl(CAN1,2,-ExchangeVel(1.1));
					}
					else 
					{
						VelCrl(CAN1,1,ExchangeVel(1.1));
						VelCrl(CAN1,2,-ExchangeVel(0.9));
					}
					mode=4;
				}			
			}
			else if(crashMod==3)                   //与车相撞
			{
				if(keyWolkingStatus==4||keyWolkingStatus==5||keyWolkingStatus==6)
				{
					if(carCrash==0||carCrash==1||carCrash==2)
					{
						if(mode==0)
						{
							if(vTime<210)
							{
								VelCrl(CAN1,1,ExchangeVel(0.8));
								VelCrl(CAN1,2,-ExchangeVel(1.2));
							}
							else 
							{
								VelCrl(CAN1,1,-ExchangeVel(1.0));
								VelCrl(CAN1,2,ExchangeVel(1.2));
							}
						}
						else 
						{
							if(vTime<210)
							{
								VelCrl(CAN1,1,ExchangeVel(1.0));
								VelCrl(CAN1,2,-ExchangeVel(0.7));
							}
							else 
							{
								VelCrl(CAN1,1,-ExchangeVel(1.0));
								VelCrl(CAN1,2,ExchangeVel(0.8));
							}
						}
					}
					else
					{
						VelCrl(CAN1,1,ExchangeVel(1.1));
						VelCrl(CAN1,2,-ExchangeVel(0.7));
					}
				}
				else
				{
					if(carCrash==0||carCrash==1||carCrash==2)
					{
						if(mode==0)
						{
							if(vTime<210)
							{
								VelCrl(CAN1,1,ExchangeVel(1.2));
								VelCrl(CAN1,2,-ExchangeVel(0.8));
							}
							else 
							{
								VelCrl(CAN1,1,-ExchangeVel(1.2));
								VelCrl(CAN1,2,ExchangeVel(1.0));
							}
						}
						else 
						{
							if(vTime<210)
							{
								VelCrl(CAN1,1,ExchangeVel(0.7));
								VelCrl(CAN1,2,-ExchangeVel(1.0));
							}
							else 
							{
								VelCrl(CAN1,1,-ExchangeVel(0.8));
								VelCrl(CAN1,2,ExchangeVel(1.0));
							}
						}
					}
					else
					{
						VelCrl(CAN1,1,ExchangeVel(0.7));
						VelCrl(CAN1,2,-ExchangeVel(1.1));
					}
				}
			}
			usartCrashMod=crashMod;
		}
		if(vTime>=300)
		{
			goBackFlag=0;
			vTime=0;
			if(x>-1700&&x<1700&&y>800&&y<4000)
			{
				correctFlag=1;
			}
			else correctFlag=0;
		}
	}
	if(vTrue>0.3&&goBackFlag==0&&vTime>0)
	{
		vTime=0;
	}
}

/**
* @brief 扫边函数
* @param 	速度 单位	m/s
* @note 	modeClearFlag为0扫小圈，为1扫大圈
*/
/*逆时针扫边*/
void clearEdge(float v,int modeClearFlag)
{
	if(modeClearFlag==0)                    //扫小边
	{
		static int modeFlag;                  
		if(squareFlag==0)
		{
			modeFlag=1;
			squareFlag=1;
			if(keyRedBlueStatus==1)
			{
				if(redBallStore==2)
				{
					modeFlag=3;
				}
				if(redBallStore==5)
				{
					modeFlag=1;
				}
			}
			if(keyRedBlueStatus==0)
			{
				if(blueBallStore==4)
				{
					modeFlag=0;
				}		
				if(blueBallStore==2||blueBallStore==5)
				{
					modeFlag=1;
				}	
				if(blueBallStore==6)
				{
					modeFlag=3;
				}
			}
		}                               
		int setAngle;
		static struct PID distancePID_t={0.115,0,0};     //0.1  950
		if(modeFlag==0)
		{
			setAngle=-90;
			distancePID_t.nowErr=y-1780;     //判定距离直线上方为正，下方为负
			distancePID_t.kp=0.115;
			distancePID_t.outPut=distancePID_t.kp*distancePID_t.nowErr;
			if(distancePID_t.outPut>90)
			{
				distancePID_t.outPut=90;
			}
			if(distancePID_t.outPut<-90)
			{
				distancePID_t.outPut=-90;
			}
			setAngle=setAngle-(int)distancePID_t.outPut;
			if(x>30)
			{
				modeFlag=10;
			}
		}
		if(modeFlag==10)
		{
			VelCrl(CAN1, 1,-ExchangeVel(0.2));
			VelCrl(CAN1, 2,ExchangeVel(1.4));	
			if(angle>-10)
			{
				modeFlag=1;
			}
		}
		if(modeFlag==1)
		{
			setAngle=0;
			distancePID_t.nowErr=x-550;     //判定距离直线左边为负，右边为正
			distancePID_t.kp=0.115;
			distancePID_t.outPut=distancePID_t.kp*distancePID_t.nowErr;
			if(distancePID_t.outPut>90)
			{
				distancePID_t.outPut=90;
			}
			if(distancePID_t.outPut<-90)
			{
				distancePID_t.outPut=-90;
			}
			setAngle=setAngle+(int)distancePID_t.outPut;
			if(y>2380)
			{
				modeFlag=20;
			}
		}
		if(modeFlag==20)
		{
			VelCrl(CAN1, 1,-ExchangeVel(0.2));
			VelCrl(CAN1, 2,ExchangeVel(1.4));	
			if(angle>80)
			{
				modeFlag=2;
			}
		}
		if(modeFlag==2)
		{
			setAngle=90;
			distancePID_t.nowErr=y-2950;     	//判定距离直线上方为正，下方为负
			distancePID_t.kp=0.1;
			distancePID_t.outPut=distancePID_t.kp*distancePID_t.nowErr;
			if(distancePID_t.outPut>90)
			{
				distancePID_t.outPut=90;
			}
			if(distancePID_t.outPut<-90)
			{
				distancePID_t.outPut=-90;
			}
			setAngle=setAngle+(int)distancePID_t.outPut;
			if(x<-30)
			{
				modeFlag=30;
			}
		}
		if(modeFlag==30)
		{
			VelCrl(CAN1, 1,-ExchangeVel(0.2));
			VelCrl(CAN1, 2,ExchangeVel(1.4));	
			if(angle>170)
			{
				modeFlag=3;
			}
		}
		if(modeFlag==3)
		{
			setAngle=180;
			distancePID_t.nowErr=x+550;   		//判定距离直线左边为负，右边为正
			distancePID_t.kp=0.1;
			distancePID_t.outPut=distancePID_t.kp*distancePID_t.nowErr;
			if(distancePID_t.outPut>90)
			{
				distancePID_t.outPut=90;
			}
			if(distancePID_t.outPut<-90)
			{
				distancePID_t.outPut=-90;
			}
			setAngle=setAngle-(int)distancePID_t.outPut;
			if(y<2250)
			{
				modeFlag=40;
			}
		}
		if(modeFlag==40)
		{
			VelCrl(CAN1, 1,-ExchangeVel(0.2));
			VelCrl(CAN1, 2,ExchangeVel(1.4));	
			if(angle>-100&&angle<-10)
			{
				modeFlag=0;
			}
		}
		static struct PID PidAngle_t={0.02,0,0};     //0.025
		PidAngle_t.nowErr=setAngle-angle;
	//	PidAngle_t.errSum+=PidAngle_t.nowErr;
	//	PidAngle_t.dErr=PidAngle_t.nowErr-PidAngle_t.lastErr;
		if(PidAngle_t.nowErr<-180)
		{
			angle=angle-360;
			PidAngle_t.nowErr=setAngle-angle;
		}
		if(PidAngle_t.nowErr>180)
		{
			angle=angle+360;
			PidAngle_t.nowErr=setAngle-angle;
		}
		PidAngle_t.outPut=PidAngle_t.kp*PidAngle_t.nowErr+PidAngle_t.ki*PidAngle_t.errSum+PidAngle_t.kd*PidAngle_t.dErr;
	//	PidAngle_t.lastErr=PidAngle_t.nowErr;
		/*角速度限制小于200度*/
		if(PidAngle_t.outPut>((200/180.f)*PI*0.17))
		{
			PidAngle_t.outPut=(200/180.f)*PI*0.17;
		}
		if(PidAngle_t.outPut<-((200/180.f)*PI*0.17))
		{
			PidAngle_t.outPut=-(200/180.f)*PI*0.17;
		}	
		if(modeFlag==10||modeFlag==20||modeFlag==30||modeFlag==40)
		{
			
		}
		else
		{
		VelCrl(CAN1, 1,-ExchangeVel(v-PidAngle_t.outPut));
		VelCrl(CAN1, 2,ExchangeVel(v+PidAngle_t.outPut));	
		}
	}
	else   
	{
		static int modeFlag=0; 
		usartModFlag=modeFlag;		
		if(squareFlag==0)
		{
			
			modeFlag=1;
			squareFlag=1;
			if(keyRedBlueStatus==1)
			{
				if(redBallStore==2)
				{
					modeFlag=3;
				}
				if(redBallStore==5)
				{
					modeFlag=1;
				}
			}
			if(keyRedBlueStatus==0)
			{
				if(blueBallStore==4)
				{
					modeFlag=0;
				}		
				if(blueBallStore==2||blueBallStore==5)
				{
					modeFlag=1;
				}	
				if(blueBallStore==6)
				{
					modeFlag=3;
				}
			}
		}                               
		int setAngle;
		static struct PID distancePID_t={0.08,0,0};     //0.1  950
		if(modeFlag==0)
		{
			setAngle=-90;
			distancePID_t.nowErr=y-200;     //判定距离直线上方为正，下方为负
			distancePID_t.outPut=distancePID_t.kp*distancePID_t.nowErr;
			if(distancePID_t.outPut>90)
			{
				distancePID_t.outPut=90;
			}
			if(distancePID_t.outPut<-90)
			{
				distancePID_t.outPut=-90;
			}
			setAngle=setAngle-(int)distancePID_t.outPut;
			if(x>1140)
			{
				modeFlag=10;
			}
		}
		if(modeFlag==10)
		{
			VelCrl(CAN1, 1,-ExchangeVel(0.15));
			VelCrl(CAN1, 2,ExchangeVel(1.8));	
			if(angle>-20)
			{
				modeFlag=1;
			}
		}
		if(modeFlag==1)
		{
			setAngle=0;
			distancePID_t.nowErr=x-2150;     //判定距离直线左边为负，右边为正
			distancePID_t.outPut=distancePID_t.kp*distancePID_t.nowErr;
			if(distancePID_t.outPut>90)
			{
				distancePID_t.outPut=90;
			}
			if(distancePID_t.outPut<-90)
			{
				distancePID_t.outPut=-90;
			}
			setAngle=setAngle+(int)distancePID_t.outPut;
			if(y>3370)
			{
				modeFlag=20;
			}
		}
		if(modeFlag==20)
		{
			VelCrl(CAN1, 1,-ExchangeVel(0.15));
			VelCrl(CAN1, 2,ExchangeVel(1.8));	
			if(angle>70)
			{
				modeFlag=2;
			}
		}
		if(modeFlag==2)
		{
			setAngle=90;
			distancePID_t.nowErr=y-4350;     	//判定距离直线上方为正，下方为负
			distancePID_t.outPut=distancePID_t.kp*distancePID_t.nowErr;
			if(distancePID_t.outPut>90)
			{
				distancePID_t.outPut=90;
			}
			if(distancePID_t.outPut<-90)
			{
				distancePID_t.outPut=-90;
			}
			setAngle=setAngle+(int)distancePID_t.outPut;
			if(x<-1110)
			{
				modeFlag=30;
			}
		}
		if(modeFlag==30)
		{
			VelCrl(CAN1, 1,-ExchangeVel(0.15));
			VelCrl(CAN1, 2,ExchangeVel(1.8));	
			if(angle>160)
			{
				modeFlag=3;
			}
		}
		if(modeFlag==3)
		{
			setAngle=180;
			distancePID_t.nowErr=x+2150;   		//判定距离直线左边为负，右边为正
			distancePID_t.outPut=distancePID_t.kp*distancePID_t.nowErr;
			if(distancePID_t.outPut>90)
			{
				distancePID_t.outPut=90;
			}
			if(distancePID_t.outPut<-90)
			{
				distancePID_t.outPut=-90;
			}
			setAngle=setAngle-(int)distancePID_t.outPut;
			if(y<1180)
			{
				modeFlag=40;
			}
		}
		if(modeFlag==40)
		{
			VelCrl(CAN1, 1,-ExchangeVel(0.15));
			VelCrl(CAN1, 2,ExchangeVel(1.8));	
			if(angle>-110&&angle<-10)
			{
				modeFlag=0;
			}
		}
		static struct PID PidAngle_t={0.025,0,0.1};     //0.025   //原始数据 0.012
		PidAngle_t.nowErr=setAngle-angle;
	//	PidAngle_t.errSum+=PidAngle_t.nowErr;
	//	PidAngle_t.dErr=PidAngle_t.nowErr-PidAngle_t.lastErr;
		if(PidAngle_t.nowErr<-180)
		{
			angle=angle-360;
			PidAngle_t.nowErr=setAngle-angle;
		}
		if(PidAngle_t.nowErr>180)
		{
			angle=angle+360;
			PidAngle_t.nowErr=setAngle-angle;
		}
		PidAngle_t.outPut=PidAngle_t.kp*PidAngle_t.nowErr+PidAngle_t.ki*PidAngle_t.errSum+PidAngle_t.kd*PidAngle_t.dErr;
	//	PidAngle_t.lastErr=PidAngle_t.nowErr;
		/*角速度限制小于200度*/
		if(PidAngle_t.outPut>((200/180.f)*PI*0.17))
		{
			PidAngle_t.outPut=(200/180.f)*PI*0.17;
		}
		if(PidAngle_t.outPut<-((200/180.f)*PI*0.17))
		{
			PidAngle_t.outPut=-(200/180.f)*PI*0.17;
		}					
		VelCrl(CAN1, 1,-ExchangeVel(v-PidAngle_t.outPut));
		VelCrl(CAN1, 2,ExchangeVel(v+PidAngle_t.outPut));	
		
	}
}
/*顺时针扫边0扫小边，1扫大边*/
void clearEdgeWise(float v,int modeClearFlag)
{
	if(modeClearFlag==0)                    //扫小边
	{
		static int modeFlag;                  
		if(squareFlag==0)
		{
			modeFlag=1;
			squareFlag=1;
		}                               
		int setAngle;
		static struct PID distancePID_t={0.115,0,0};     //0.1  950
		if(modeFlag==0)
		{
			setAngle=90;
			distancePID_t.nowErr=y-1780;     //判定距离直线上方为正，下方为负
			distancePID_t.kp=0.115;
			distancePID_t.outPut=distancePID_t.kp*distancePID_t.nowErr;
			if(distancePID_t.outPut>90)
			{
				distancePID_t.outPut=90;
			}
			if(distancePID_t.outPut<-90)
			{
				distancePID_t.outPut=-90;
			}
			setAngle=setAngle+(int)distancePID_t.outPut;
			if(x<-30)
			{
				modeFlag=10;
			}
		}
		if(modeFlag==10)
		{
			VelCrl(CAN1, 1,-ExchangeVel(1.4));
			VelCrl(CAN1, 2,ExchangeVel(0.2));	
			if(angle<10)
			{
				modeFlag=1;
			}
		}
		if(modeFlag==1)
		{
			setAngle=0;
			distancePID_t.nowErr=x+550;     //判定距离直线左边为负，右边为正
			distancePID_t.kp=0.115;
			distancePID_t.outPut=distancePID_t.kp*distancePID_t.nowErr;
			if(distancePID_t.outPut>90)
			{
				distancePID_t.outPut=90;
			}
			if(distancePID_t.outPut<-90)
			{
				distancePID_t.outPut=-90;
			}
			setAngle=setAngle+(int)distancePID_t.outPut;
			if(y>2380)
			{
				modeFlag=20;
			}
		}
		if(modeFlag==20)
		{
			VelCrl(CAN1, 1,-ExchangeVel(1.4));
			VelCrl(CAN1, 2,ExchangeVel(0.2));	
			if(angle<-80)
			{
				modeFlag=2;
			}
		}
		if(modeFlag==2)
		{
			setAngle=-90;
			distancePID_t.nowErr=y-2950;     	//判定距离直线上方为正，下方为负
			distancePID_t.kp=0.1;
			distancePID_t.outPut=distancePID_t.kp*distancePID_t.nowErr;
			if(distancePID_t.outPut>90)
			{
				distancePID_t.outPut=90;
			}
			if(distancePID_t.outPut<-90)
			{
				distancePID_t.outPut=-90;
			}
			setAngle=setAngle-(int)distancePID_t.outPut;
			if(x>30)
			{
				modeFlag=30;
			}
		}
		if(modeFlag==30)
		{
			VelCrl(CAN1, 1,-ExchangeVel(1.4));
			VelCrl(CAN1, 2,ExchangeVel(0.2));	
			if(angle<-170)
			{
				modeFlag=3;
			}
		}
		if(modeFlag==3)
		{
			setAngle=180;
			distancePID_t.nowErr=x-550;   		//判定距离直线左边为负，右边为正
			distancePID_t.kp=0.1;
			distancePID_t.outPut=distancePID_t.kp*distancePID_t.nowErr;
			if(distancePID_t.outPut>90)
			{
				distancePID_t.outPut=90;
			}
			if(distancePID_t.outPut<-90)
			{
				distancePID_t.outPut=-90;
			}
			setAngle=setAngle-(int)distancePID_t.outPut;
			if(y<2250)
			{
				modeFlag=40;
			}
		}
		if(modeFlag==40)
		{
			VelCrl(CAN1, 1,-ExchangeVel(1.4));
			VelCrl(CAN1, 2,ExchangeVel(0.2));	
			if(angle<100&&angle>10)
			{
				modeFlag=0;
			}
		}
		static struct PID PidAngle_t={0.02,0,0};     //0.025
		PidAngle_t.nowErr=setAngle-angle;
	//	PidAngle_t.errSum+=PidAngle_t.nowErr;
	//	PidAngle_t.dErr=PidAngle_t.nowErr-PidAngle_t.lastErr;
		if(PidAngle_t.nowErr<-180)
		{
			angle=angle-360;
			PidAngle_t.nowErr=setAngle-angle;
		}
		if(PidAngle_t.nowErr>180)
		{
			angle=angle+360;
			PidAngle_t.nowErr=setAngle-angle;
		}
		PidAngle_t.outPut=PidAngle_t.kp*PidAngle_t.nowErr+PidAngle_t.ki*PidAngle_t.errSum+PidAngle_t.kd*PidAngle_t.dErr;
	//	PidAngle_t.lastErr=PidAngle_t.nowErr;
		/*角速度限制小于200度*/
		if(PidAngle_t.outPut>((200/180.f)*PI*0.17))
		{
			PidAngle_t.outPut=(200/180.f)*PI*0.17;
		}
		if(PidAngle_t.outPut<-((200/180.f)*PI*0.17))
		{
			PidAngle_t.outPut=-(200/180.f)*PI*0.17;
		}	
		if(modeFlag==10||modeFlag==20||modeFlag==30||modeFlag==40)
		{
			
		}
		else
		{
		VelCrl(CAN1, 1,-ExchangeVel(v-PidAngle_t.outPut));
		VelCrl(CAN1, 2,ExchangeVel(v+PidAngle_t.outPut));	
		}
		usartModFlag=modeFlag;
	} 
	else   
	{
		static int modeFlag=1;                  
		static int squareFlag=0;
		if(squareFlag==0)
		{
			if(keyWolkingStatus==1)
			{
				if(redBallStore==1)	modeFlag=2;
				if(redBallStore==2) //
				if(redBallStore==3) modeFlag=1;
				if(redBallStore==4) modeFlag=3;
				if(redBallStore==5) //
				if(redBallStore==6) modeFlag=0;
			}
			if(keyWolkingStatus==0) //BLUE
			{
				if(blueBallStore==1)	modeFlag=0;
				if(blueBallStore==2) //
				if(blueBallStore==3) modeFlag=3;  
				if(blueBallStore==4) modeFlag=1;
				if(blueBallStore==5) //
				if(blueBallStore==6) modeFlag=2;
			}
      squareFlag=1;
		}                               
		int setAngle;
		static struct PID distancePID_t={0.08,0,0};     //0.1  950
		if(modeFlag==0)
		{
			setAngle=90;
			distancePID_t.nowErr=y-300;     //判定距离直线上方为正，下方为负
			distancePID_t.outPut=distancePID_t.kp*distancePID_t.nowErr;
			if(distancePID_t.outPut>90)
			{
				distancePID_t.outPut=90;
			}
			if(distancePID_t.outPut<-90)
			{
				distancePID_t.outPut=-90;
			}
			setAngle=setAngle+(int)distancePID_t.outPut;
			if(x<-1040)
			{
				modeFlag=10;
			}
		}
		if(modeFlag==10)
		{
			VelCrl(CAN1, 1,-ExchangeVel(1.8));
			VelCrl(CAN1, 2,ExchangeVel(0.15));	
			if(angle<20)
			{
				modeFlag=1;
			}
		}
		if(modeFlag==1)
		{
			setAngle=0;
			distancePID_t.nowErr=x+2050;     //判定距离直线左边为负，右边为正
			distancePID_t.outPut=distancePID_t.kp*distancePID_t.nowErr;
			if(distancePID_t.outPut>90)
			{
				distancePID_t.outPut=90;
			}
			if(distancePID_t.outPut<-90)
			{
				distancePID_t.outPut=-90;
			}
			setAngle=setAngle+(int)distancePID_t.outPut;
			if(y>3270)
			{
				modeFlag=20;
			}
		}
		if(modeFlag==20)
		{
			VelCrl(CAN1, 1,-ExchangeVel(1.8));
			VelCrl(CAN1, 2,ExchangeVel(0.15));	
			if(angle<-70)
			{
				modeFlag=2;
			}
		}
		if(modeFlag==2)
		{
			setAngle=-90;
			distancePID_t.nowErr=y-4250;     	//判定距离直线上方为正，下方为负
			distancePID_t.outPut=distancePID_t.kp*distancePID_t.nowErr;
			if(distancePID_t.outPut>90)
			{
				distancePID_t.outPut=90;
			}
			if(distancePID_t.outPut<-90)
			{
				distancePID_t.outPut=-90;
			}
			setAngle=setAngle-(int)distancePID_t.outPut;
			if(x>1010)
			{
				modeFlag=30;
			}
		}
		if(modeFlag==30)
		{
			VelCrl(CAN1, 1,-ExchangeVel(1.8));
			VelCrl(CAN1, 2,ExchangeVel(0.15));	
			if(angle<-160)
			{
				modeFlag=3;
			}
		}
		if(modeFlag==3)
		{
			setAngle=180;
			distancePID_t.nowErr=x-2050;   		//判定距离直线左边为负，右边为正
			distancePID_t.outPut=distancePID_t.kp*distancePID_t.nowErr;
			if(distancePID_t.outPut>90)
			{
				distancePID_t.outPut=90;
			}
			if(distancePID_t.outPut<-90)
			{
				distancePID_t.outPut=-90;
			}
			setAngle=setAngle-(int)distancePID_t.outPut;
			if(y<1280)
			{
				modeFlag=40;
			}
		}
		if(modeFlag==40)
		{
			VelCrl(CAN1, 1,-ExchangeVel(1.8));
			VelCrl(CAN1, 2,ExchangeVel(0.15));	
			if(angle<110&&angle>10)
			{
				modeFlag=0;
			}
		}
		static struct PID PidAngle_t={0.025,0,0.1};     //0.025   //原始数据 0.012
		PidAngle_t.nowErr=setAngle-angle;
	//	PidAngle_t.errSum+=PidAngle_t.nowErr;
	//	PidAngle_t.dErr=PidAngle_t.nowErr-PidAngle_t.lastErr;
		if(PidAngle_t.nowErr<-180)
		{
			angle=angle-360;
			PidAngle_t.nowErr=setAngle-angle;
		}
		if(PidAngle_t.nowErr>180)
		{
			angle=angle+360;
			PidAngle_t.nowErr=setAngle-angle;
		}
		PidAngle_t.outPut=PidAngle_t.kp*PidAngle_t.nowErr+PidAngle_t.ki*PidAngle_t.errSum+PidAngle_t.kd*PidAngle_t.dErr;
	//	PidAngle_t.lastErr=PidAngle_t.nowErr;
		/*角速度限制小于200度*/
		if(PidAngle_t.outPut>((200/180.f)*PI*0.17))
		{
			PidAngle_t.outPut=(200/180.f)*PI*0.17;
		}
		if(PidAngle_t.outPut<-((200/180.f)*PI*0.17))
		{
			PidAngle_t.outPut=-(200/180.f)*PI*0.17;
		}					
		if(modeFlag==10||modeFlag==20||modeFlag==30||modeFlag==40)
		{
			
		}
		else
		{
			VelCrl(CAN1, 1,-ExchangeVel(v-PidAngle_t.outPut));
			VelCrl(CAN1, 2,ExchangeVel(v+PidAngle_t.outPut));	
		}
			usartModFlag=modeFlag;
	}
}

/**
* @brief	 	圆形闭环(串级)
* @param		速度 m/s 
* @param		半径 mm  
* @param		模式1顺0逆  
* @note			无
* @note   	
* @note			
*/
void PidRound(float v0, float r ,int modePidRound) 
{
	float v=v0*1000;
	float v1,v2;
	float angleGoal;
	int pulse1,pulse2;
	static struct PID PidRouDistan_t={0,0,0};
	static struct PID PidRouAngle_t={6,0,60};
	if(x==0)                                        //四个特殊点的angleGoal
	{
		if(y<2400)
		{
			angleGoal=-90;
		}
		else angleGoal=90;
	}
	else if(y==2400)
	{
		if(x>0)
		{
			angleGoal=0; 
		}
		else angleGoal=180;
	}
	else                                           //一般情况下的angleGoal
	{
		angleGoal=atan((y-2400)/x)*180/PI;
		if(x<0)
		{
			if(y>2400)
			{
				angleGoal=angleGoal+180;
			}
			if(y<2400)
			{
				angleGoal=angleGoal-180;
			}
		}
	}
	if(modePidRound==1)												//顺时针走圆的时候，angleGoal与逆时针的有一定关系
	{
		if(y<2400)
		{
			angleGoal=angleGoal+180;
		}
		else angleGoal=angleGoal-180;
	}
	if(angleGoal>0&&angle<angleGoal-180)			//保证走劣弧
	{
		angle=angle+360;
	}
	else if(angleGoal<0&&angle>angleGoal+180)
	{            
		angle=angle-360;
	}
	PidRouDistan_t.nowErr=r-sqrt(x*x+(y-2400)*(y-2400));
	PidRouDistan_t.errSum+=PidRouDistan_t.nowErr;
	PidRouDistan_t.dErr=PidRouDistan_t.nowErr-PidRouDistan_t.lastErr;
//	PidRouDistan_t.outPut=PidRouDistan_t.kp*PidRouDistan_t.nowErr+PidRouDistan_t.ki*PidRouDistan_t.errSum+PidRouDistan_t.kd*PidRouDistan_t.dErr;
	PidRouDistan_t.outPut=atan(PidRouDistan_t.nowErr/100.0)*180/PI;
	
	if(modePidRound==0)																			
	{		
		PidRouAngle_t.nowErr=angleGoal-angle-PidRouDistan_t.outPut;
		PidRouAngle_t.dErr=PidRouAngle_t.nowErr-PidRouAngle_t.lastErr;
		PidRouAngle_t.errSum+=PidRouAngle_t.nowErr;	
		PidRouAngle_t.outPut=PidRouAngle_t.kp*PidRouAngle_t.nowErr+PidRouAngle_t.ki*PidRouAngle_t.errSum+PidRouAngle_t.kd*PidRouAngle_t.dErr;
		v1=(r-170)/r*v-PidRouAngle_t.outPut;
		v2=(r+170)/r*v+PidRouAngle_t.outPut;
	}
	if(modePidRound==1)
	{
		PidRouAngle_t.nowErr=angleGoal-angle+PidRouDistan_t.outPut;
		PidRouAngle_t.dErr=PidRouAngle_t.nowErr-PidRouAngle_t.lastErr;
		PidRouAngle_t.errSum+=PidRouAngle_t.nowErr;	
		PidRouAngle_t.outPut=PidRouAngle_t.kp*PidRouAngle_t.nowErr+PidRouAngle_t.ki*PidRouAngle_t.errSum+PidRouAngle_t.kd*PidRouAngle_t.dErr;
		v1=(r+170)/r*v-PidRouAngle_t.outPut;
		v2=(r-170)/r*v+PidRouAngle_t.outPut;
	}
	pulse1=(int)(v1/(120*PI)*32768);
	pulse2=(int)(v2/(120*PI)*32768);
	VelCrl(CAN1, 1,-pulse1);
	VelCrl(CAN1, 2,pulse2);
	vCheckL=-pulse1;
	vCheckR=pulse2;
	PidRouDistan_t.lastErr=PidRouDistan_t.nowErr;
	PidRouAngle_t.lastErr=PidRouAngle_t.nowErr;
}

void PidBiggerRoundOri(float v0, float r ,int modePidRound) 
{
	static int roundFlag=0; 
	float v=v0*1000;
	float v1,v2;
	float angleGoal; 
	int pulse1,pulse2;
	static struct PID PidRouDistan_t={0,0,20};  //1.3,0,50,1.8,0,50
	static struct PID PidRouAngle_t={0,0,10};
	if(x>-1&&x<1)                                        //四个特殊点的angleGoal
	{
		if(y<2325)
		{
			angleGoal=-90;
		}
		else angleGoal=90;
	}
	else if(y<2326&&y>2324)
	{
		if(x>0)
		{
			angleGoal=0; 
		}
		else angleGoal=180;
	}
	else                                           //一般情况下的angleGoal
	{
		angleGoal=atan2((y-2325)*1.0,x*1.0)*180/PI;
	}
	if(modePidRound==1)												//顺时针走圆的时候，angleGoal与逆时针的有一定关系
	{
		if(y<2325)
		{
			angleGoal=angleGoal+180;
		}
		else angleGoal=angleGoal-180;
	}
	if(angleGoal>0&&angle<angleGoal-180)			//保证走劣弧
	{
		angle=angle+360;
	}
	else if(angleGoal<0&&angle>angleGoal+180)
	{           
		angle=angle-360;
	}
	/*PID参数调节*/
	PidRouDistan_t.nowErr=r+detr1-sqrt(x*x+(y-2325)*(y-2325));
	PidRouDistan_t.dErr=PidRouDistan_t.nowErr-PidRouDistan_t.lastErr;
	PidRouDistan_t.kp=(900-170.0/(r+detr1)*(v+detv1))/(2325-r);
	PidRouDistan_t.outPut=PidRouDistan_t.kp*PidRouDistan_t.nowErr+PidRouDistan_t.kd*PidRouDistan_t.dErr;
	if(modePidRound==1)
	{		
		PidRouAngle_t.nowErr=angleGoal-angle-3;					//PID参数
	}
	else
	{
		PidRouAngle_t.nowErr=angleGoal-angle+3;
	}
	if(fabs(PidRouDistan_t.nowErr)>600)
	{
		PidRouAngle_t.kp=36;
	}
	else
	{
		PidRouAngle_t.kp=28 ;
	}
	PidRouAngle_t.dErr=PidRouAngle_t.nowErr-PidRouAngle_t.lastErr;
	PidRouAngle_t.outPut=PidRouAngle_t.kp*PidRouAngle_t.nowErr+PidRouAngle_t.kd*PidRouAngle_t.dErr;
	if(modePidRound==0)																				//限幅
	{		
		if(PidRouDistan_t.outPut-PidRouAngle_t.outPut>800)
		{
			PidRouDistan_t.outPut=800;
			PidRouAngle_t.outPut=0;
		}
		if(PidRouDistan_t.outPut-PidRouAngle_t.outPut<-800)
		{
			PidRouDistan_t.outPut=-800;
			PidRouAngle_t.outPut=0;
		}
		v1=(r+detr1-170)/(r+detr1)*(v+detv1)+PidRouDistan_t.outPut-PidRouAngle_t.outPut;
		v2=(r+detr1+170)/(r+detr1)*(v+detv1)-PidRouDistan_t.outPut+PidRouAngle_t.outPut;
	}
	if(modePidRound==1)
	{
		if(PidRouDistan_t.outPut+PidRouAngle_t.outPut>800)
		{
			PidRouDistan_t.outPut=800;
			PidRouAngle_t.outPut=0;
		}
		if(PidRouDistan_t.outPut+PidRouAngle_t.outPut<-800)
		{
			PidRouDistan_t.outPut=-800;
			PidRouAngle_t.outPut=0;
		}
		v1=(r+detr1+170)/(r+detr1)*(v+detv1)-PidRouDistan_t.outPut-PidRouAngle_t.outPut; 
		v2=(r+detr1-170)/(r+detr1)*(v+detv1)+PidRouDistan_t.outPut+PidRouAngle_t.outPut;
	}
	pulse1=(int)(v1/(120*PI)*32768);
	pulse2=(int)(v2/(120*PI)*32768);
	VelCrl(CAN1, 1,-pulse1);
	VelCrl(CAN1, 2,pulse2);
	vCheckL=-pulse1;
	vCheckR=pulse2;
	PidRouDistan_t.lastErr=PidRouDistan_t.nowErr;
	PidRouAngle_t.lastErr=PidRouAngle_t.nowErr;
	if(modePidRound==0)   //逆时针保证最上方半径变大
	{
		if(x>10)
		{
			roundFlag=1;
		}
		if(x<0&&roundFlag==1)
		{
				detr1+=350;
				detv1+=120;
				roundFlag=0;
		}
		if(detr1>1050)
		{
			detr1-=350;
			detv1-=120;
			mode=2;
		}
		if(detr1<-1050)
		{
			detr1+=350;
			detv1+=120;
			mode=3;
		}
	}
	else                        //顺时针保证最上方半径变大
	{
		if(x<-10)
		{
			roundFlag=1;
		}
		if(x>0&&roundFlag==1)
		{
				detr1+=350;
				detv1+=120;
				roundFlag=0;
		}
		if(detr1>1050)
		{
			detr1-=350;
			detv1-=120;
			mode=2;
		}
		if(detr1<-1050)
		{
			detr1+=350;
			detv1+=120;
			mode=3;
		}
	}
}
void PidBiggerSmallerRoundOri(float v0, float r ,int modePidRound)
{
	float v=v0*1000;
	float v1,v2;
	float angleGoal; 
	int pulse1,pulse2;
	static struct PID PidRouDistan_t={0.08,0,0};  //1.3,0,50,1.8,0,50
	static struct PID PidRouAngle_t={50,0,0};
	if(x>-1&&x<1)                                        //四个特殊点的angleGoal
	{
		if(y<2325)
		{
			angleGoal=-90;
		}
		else angleGoal=90;
	}
	else if(y<2326&&y>2324)
	{
		if(x>0)
		{
			angleGoal=0; 
		}
		else angleGoal=180;
	}
	else                                           //一般情况下的angleGoal
	{
		angleGoal=atan2((y-2325)*1.0,x*1.0)*180/PI;
	}
	if(modePidRound==1)												//顺时针走圆的时候，angleGoal与逆时针的有一定关系
	{
		if(y<2325)
		{
			angleGoal=angleGoal+180;
		}
		else angleGoal=angleGoal-180;
	}
	if(angleGoal>0&&angle<angleGoal-180)			//保证走劣弧
	{
		angle=angle+360;
	}
	else if(angleGoal<0&&angle>angleGoal+180)
	{            
		angle=angle-360;
	}
	/*PID参数调节*/
	PidRouDistan_t.nowErr=r-sqrt(x*x+(y-2325)*(y-2325));
	PidRouDistan_t.outPut=PidRouDistan_t.kp*PidRouDistan_t.nowErr;
	if(PidRouDistan_t.outPut>90)
	{
		PidRouDistan_t.outPut=90;
	}
	if(PidRouDistan_t.outPut<-90)
	{
		PidRouDistan_t.outPut=-90;
	}
	if(modePidRound==0)	
	{
		angleGoal=angleGoal-PidRouDistan_t.outPut;
	}
	if(modePidRound==1)
	{
		angleGoal=angleGoal+PidRouDistan_t.outPut;
	}		
	PidRouAngle_t.nowErr=angleGoal-angle;
	PidRouAngle_t.outPut=PidRouAngle_t.kp*PidRouAngle_t.nowErr;
	if(modePidRound==0)																				//限幅
	{		
		if(PidRouAngle_t.outPut>800)
		{
			PidRouAngle_t.outPut=800;
		}
		if(PidRouAngle_t.outPut<-800)
		{
			PidRouAngle_t.outPut=-800;
		}
//		v1=v-PidRouAngle_t.outPut;
//		v1=v+PidRouAngle_t.outPut;
		v1=(r-170)/(r)*v-PidRouAngle_t.outPut;
		v2=(r+170)/(r)*v+PidRouAngle_t.outPut;
	}
	if(modePidRound==1)
	{
		if(PidRouAngle_t.outPut>800)
		{
			PidRouAngle_t.outPut=800;
		}
		if(PidRouAngle_t.outPut<-800)
		{
			PidRouAngle_t.outPut=-800;
		}
//		v1=v-PidRouAngle_t.outPut;
//		v1=v+PidRouAngle_t.outPut;
		v1=(r+170)/(r)*v-PidRouAngle_t.outPut; 
		v2=(r-170)/(r)*v+PidRouAngle_t.outPut;
	}
	pulse1=(int)(v1/(120*PI)*32768);
	pulse2=(int)(v2/(120*PI)*32768);
	VelCrl(CAN1, 1,-pulse1);
	VelCrl(CAN1, 2,pulse2);
	static int circleFlag=0,circleTimes;	
	if(x>10) circleFlag=1;
	if(x<0&&circleFlag==1)
	{
		circleTimes++;
		circleFlag=0;
	}
	if(circleTimes>2)
	{
		mode=0;
		detr1=0;
		detv1=0;
		circleTimes=0;
	}
}
/**
* @brief	 	圆形闭环(be able to start from the beginning area)
* @note			无
* @param 	 	v 速度 m/s
* @param  	r 半径 mm
* @param  	modePidRound 1顺0逆
* @note   	注意把该函数与PidRound区别开来。
*/ 
void SelfCheck(void)
{
	ReadActualPos(CAN2,7); 
	static int personTimeCnt;
	if(personTimeCnt<500)
	{
		VelCrl(CAN1,1,-32768);
		VelCrl(CAN1,2,32768);
		USART_OUT(USART1,"L\t%d\t",(-1*actual1Vel));
		USART_OUT(USART1,"R\t%d\t",(-1*actual2Vel));
		USART_OUT(USART1,"shooterVel\t%d\r\n",(int)(100*shooterVel));
		ShooterVelCtrl(30);
	}
	else if(personTimeCnt<1000)
	{
		VelCrl(CAN1,1,32768);
		VelCrl(CAN1,2,-32768);
		USART_OUT(USART1,"L\t%d\t",(-1*actual1Vel));
		USART_OUT(USART1,"R\t%d\t",(-1*actual2Vel));
		USART_OUT(USART1,"shooterVel\t%d\r\n",(int)(100*shooterVel));
		ShooterVelCtrl(50);
	}
	else if(personTimeCnt<1300)
	{
		VelCrl(CAN1,1,0);
		VelCrl(CAN1,2,0);
		YawPosCtrl(30);
		PosCrl(CAN2,7,ABSOLUTE_MODE,-28168);
		USART_OUT(USART1,"ballcolor0\t%d\t",ballColor[0]);
		USART_OUT(USART1,"ballcolor1\t%d\t",ballColor[1]);
		USART_OUT(USART1,"yawPos\t%d\t",(int)(100*yawPos));
		USART_OUT(USART1,"motor:\t%d\r\n",actual7Pos);
	}
	else if(personTimeCnt<1600)
	{
		VelCrl(CAN1,1,0);
		VelCrl(CAN1,2,0);
		YawPosCtrl(-30);
		PosCrl(CAN2,7,ABSOLUTE_MODE,0);
		USART_OUT(USART1,"ballcolor0\t%d\t",ballColor[0]);
		USART_OUT(USART1,"ballcolor1\t%d\t",ballColor[1]);
		USART_OUT(USART1,"yawPos\t%d\t",(int)(100*yawPos));
		USART_OUT(USART1,"motor:\t%d\r\n",actual7Pos);
	}
	else if(personTimeCnt<1900)
	{
		VelCrl(CAN1,1,0);
		VelCrl(CAN1,2,0);
		YawPosCtrl(60);
		PosCrl(CAN2,7,ABSOLUTE_MODE,-28168);
		USART_OUT(USART1,"ballcolor0\t%d\t",ballColor[0]);
		USART_OUT(USART1,"ballcolor1\t%d\t",ballColor[1]);
		USART_OUT(USART1,"yawPos\t%d\t",(int)(100*yawPos));
		USART_OUT(USART1,"motor:\t%d\r\n",actual7Pos);
	}
	else if(personTimeCnt<2200)
	{
		VelCrl(CAN1,1,0);
		VelCrl(CAN1,2,0);
		YawPosCtrl(-60);
		PosCrl(CAN2,7,ABSOLUTE_MODE,0);
		USART_OUT(USART1,"ballcolor0\t%d\t",ballColor[0]);
		USART_OUT(USART1,"ballcolor1\t%d\t",ballColor[1]);
		USART_OUT(USART1,"yawPos\t%d\t",(int)(100*yawPos));
		USART_OUT(USART1,"motor:\t%d\r\n",actual7Pos);
	}
	else if(personTimeCnt<2500)
	{
		VelCrl(CAN1,1,0);
		VelCrl(CAN1,2,0);
		YawPosCtrl(90);
		PosCrl(CAN2,7,ABSOLUTE_MODE,-28168);
		USART_OUT(USART1,"ballcolor0\t%d\t",ballColor[0]);
		USART_OUT(USART1,"ballcolor1\t%d\t",ballColor[1]);
		USART_OUT(USART1,"yawPos\t%d\t",(int)(100*yawPos));
		USART_OUT(USART1,"motor:\t%d\r\n",actual7Pos);
	}
	else if(personTimeCnt<2800)
	{
		VelCrl(CAN1,1,0);
		VelCrl(CAN1,2,0);
		YawPosCtrl(-90);
		PosCrl(CAN2,7,ABSOLUTE_MODE,0);
		USART_OUT(USART1,"ballcolor0\t%d\t",ballColor[0]);
		USART_OUT(USART1,"ballcolor1\t%d\t",ballColor[1]);
		USART_OUT(USART1,"yawPos\t%d\t",(int)(100*yawPos));
		USART_OUT(USART1,"motor:\t%d\r\n",actual7Pos);
	}
	else
	{
		VelCrl(CAN1,1,0);
		VelCrl(CAN1,2,0);
		USART_OUT(USART1,"X:\t%d\t",(int)x);
		USART_OUT(USART1,"Y:\t%d\t",(int)y);
		USART_OUT(USART1,"XSpd:\t%d\t",(int)speedX);
		USART_OUT(USART1,"YSpd:\t%d\t",(int)speedY);
		USART_OUT(USART1,"WZ:\t%d\t",(int)angleZ);	
		USART_OUT(USART1,"Angle:\t%d\t\r\n",(int)angle);	
	}
	personTimeCnt++;
}
/*
* @brief	 	定点射击整套流程
* @note   	无
*/
extern float shootRaps;
extern int xShoot,yShoot;
extern float shootDistance;
extern int temp9;
int getBallFlag=0;    //检测走形中是否收到球
void StopAndShoot(void)
{
	if(keyRedBlueStatus==1)   //选择红方
	{
		switch(redBallStore)
		{
			case 1:  	//红方选择1、2号球仓
				switch(keyWolkingStatus)
				{
					case 1:
						RedSmallWise();
						break;
					case 2:
						RedSmallWise();
						break;
					case 3:
						RedSmallWise();
						break;
					case 4:
						RedOneTwo();
						break;
					case 5:
						RedOneTwo();
						break;
					case 6:
						RedOneTwo();
						break;
				}
				if(correctFlag==0)
				{
					angle-=2;
					if(aimActualPosFlag==3)
					{
						RedOne();
						shootDistance=fabs(sqrt((xShoot+2200)*(xShoot+2200)+(yShoot-4441)*(yShoot-4441)));
						shootRaps=(-1.949e-6*shootDistance*shootDistance)+0.02135*shootDistance+27.12-1;
						ShooterVelCtrl(shootRaps);
					}
					else if(aimActualPosFlag==4)
					{
						RedTwo();
						shootDistance=fabs(sqrt((xShoot-2200)*(xShoot-2200)+(yShoot-4441)*(yShoot-4441)));
						shootRaps=(-2.828e-6*shootDistance*shootDistance)+0.02413*shootDistance+26.53;
						ShooterVelCtrl(shootRaps);
					}
					else if(aimActualPosFlag==2)
					{
						RedOne();
						if(mode!=5)
						{
							ShooterVelCtrl(30);
						}						
						else 
						{
							shootDistance=fabs(sqrt((xShoot+2200)*(xShoot+2200)+(yShoot-4441)*(yShoot-4441)));
							shootRaps=(-1.949e-6*shootDistance*shootDistance)+0.02135*shootDistance+27.12-1;
							ShooterVelCtrl(shootRaps);
						}
					}
				}
				break;
			case 2:		//红方选择1、3号球仓
				switch(keyWolkingStatus)
				{
					case 1:
						RedSmallWise();
						break;
					case 2:
						RedSmallWise();
						break;
					case 3:
						RedSmallWise();
						break;
					case 4:
						BlueTwoFour();
						break;
					case 5:
						BlueTwoFour();
						break;
					case 6:
						BlueTwoFour();
						break;
				}
				if(correctFlag==0)
				{
					angle-=2;
					if(aimActualPosFlag==3||aimActualPosFlag==4)
					{
						if(aimActualPosFlag==3)
						{
							RedOne();
							shootDistance=fabs(sqrt((xShoot+2200)*(xShoot+2200)+(yShoot-4441)*(yShoot-4441)));
							shootRaps=(-1.949e-6*shootDistance*shootDistance)+0.02135*shootDistance+27.12;
							ShooterVelCtrl(shootRaps);
						}
						else
						{
							RedThree();
							shootDistance=fabs(sqrt((xShoot-2200)*(xShoot-2200)+(yShoot-41)*(yShoot-41)));
							shootRaps=(-1.651e-6*shootDistance*shootDistance)+0.0204*shootDistance+27.94-1;
							ShooterVelCtrl(shootRaps);
						}
					}
					else if(aimActualPosFlag==2)
					{
						RedOne();
						if(mode!=5)
						{
							ShooterVelCtrl(30);
						}						
						else 
						{
							shootDistance=fabs(sqrt((xShoot+2200)*(xShoot+2200)+(yShoot-4441)*(yShoot-4441)));
							shootRaps=(-1.949e-6*shootDistance*shootDistance)+0.02135*shootDistance+27.12;
							ShooterVelCtrl(shootRaps);
						}
					}
				}
				break;
			case 3:  	//红方选择1、4号球仓
				switch(keyWolkingStatus)
				{
					case 1:
						RedSmallWise();
						break;
					case 2:
						RedSmallWise();
						break;
					case 3:
						RedSmallWise();
						break;
					case 4:
						RedOneFour();
						break;
					case 5:
						RedOneFour();
						break;
					case 6:
						RedOneFour();
						break;
				}
				if(correctFlag==0)
				{
					angle-=2;
					if(aimActualPosFlag==3)
					{
						RedOne();
						shootDistance=fabs(sqrt((xShoot+2200)*(xShoot+2200)+(yShoot-4441)*(yShoot-4441)));
						shootRaps=(-1.949e-6*shootDistance*shootDistance)+0.02135*shootDistance+27.12-1;
						ShooterVelCtrl(shootRaps);
					}
					else if(aimActualPosFlag==4)
					{
						RedFour();
						shootDistance=fabs(sqrt((xShoot+2200)*(xShoot+2200)+(yShoot-41)*(yShoot-41)));
						shootRaps=(-1.949e-6*shootDistance*shootDistance)+0.02135*shootDistance+27.12+3-1;
						ShooterVelCtrl(shootRaps);
					}
					else if(aimActualPosFlag==2)
					{
						RedOne();
						if(mode!=5)
						{
							ShooterVelCtrl(30);
						}						
						else 
						{
							shootDistance=fabs(sqrt((xShoot+2200)*(xShoot+2200)+(yShoot-4441)*(yShoot-4441)));
							shootRaps=(-1.949e-6*shootDistance*shootDistance)+0.02135*shootDistance+27.12-1;
							ShooterVelCtrl(shootRaps);
						}
					}
				}
				break;
			case 4:  	//红方选择2、3号球仓
				switch(keyWolkingStatus)
				{
					case 1:
						RedSmallWise();
						break;
					case 2:
						RedSmallWise();
						break;
					case 3:
						RedSmallWise();
						break;
					case 4:
						RedTwoThree();
						break;
					case 5:
						RedTwoThree();
						break;
					case 6:
						RedTwoThree();
						break;
				}
				if(correctFlag==0)
				{
					angle-=2;
					if(aimActualPosFlag==3)
					{
						RedTwo();
						shootDistance=fabs(sqrt((xShoot-2200)*(xShoot-2200)+(yShoot-4441)*(yShoot-4441)));
						shootRaps=(-2.828e-6*shootDistance*shootDistance)+0.02413*shootDistance+26.53;
						ShooterVelCtrl(shootRaps);				
					}
					else if(aimActualPosFlag==4)
					{
						RedThree();
						shootDistance=fabs(sqrt((xShoot-2200)*(xShoot-2200)+(yShoot-41)*(yShoot-41)));
						shootRaps=(-1.651e-6*shootDistance*shootDistance)+0.0204*shootDistance+27.94-1;
						ShooterVelCtrl(shootRaps);
					}
					else if(aimActualPosFlag==2)
					{
						RedTwo();
						if(mode!=5)
						{
							ShooterVelCtrl(30);
						}						
						else 
						{
						shootDistance=fabs(sqrt((xShoot-2200)*(xShoot-2200)+(yShoot-4441)*(yShoot-4441)));
						shootRaps=(-2.828e-6*shootDistance*shootDistance)+0.02413*shootDistance+26.53;
						ShooterVelCtrl(shootRaps);		
						}
					}
				}
				break;
			case 5:  	//红方选择2、4号球仓
				switch(keyWolkingStatus)
				{
					case 1:
						RedSmallWise();
						break;
					case 2:
						RedSmallWise();
						break;
					case 3:
						RedSmallWise();
						break;
					case 4:
//						RedTwoFour();
					BlueTwoFour();
						break;
					case 5:
					BlueTwoFour();
						break;
					case 6:
					BlueTwoFour();
						break;
				}
				if(correctFlag==0)
				{
					angle-=2;
					if(aimActualPosFlag==3||aimActualPosFlag==4)
					{
						if(aimActualPosFlag==3)
						{
							RedTwo();
							shootDistance=fabs(sqrt((xShoot-2200)*(xShoot-2200)+(yShoot-4441)*(yShoot-4441)));
							shootRaps=(-2.828e-6*shootDistance*shootDistance)+0.02413*shootDistance+26.53;
							ShooterVelCtrl(shootRaps);
						}
						else
						{
							RedFour();
							shootDistance=fabs(sqrt((xShoot+2200)*(xShoot+2200)+(yShoot-41)*(yShoot-41)));
							shootRaps=(-1.949e-6*shootDistance*shootDistance)+0.02135*shootDistance+27.12+3-1;
							ShooterVelCtrl(shootRaps);
						}
					}
					else if(aimActualPosFlag==2)
					{
						RedTwo();
						if(mode!=5)
						{
							ShooterVelCtrl(30);
						}						
						else 
						{
						shootDistance=fabs(sqrt((xShoot-2200)*(xShoot-2200)+(yShoot-4441)*(yShoot-4441)));
						shootRaps=(-2.828e-6*shootDistance*shootDistance)+0.02413*shootDistance+26.53;
						ShooterVelCtrl(shootRaps);		
						}
					}
				}
				break;
			case 6:  	//红方选择3、4号球仓
				switch(keyWolkingStatus)
				{
					case 1:
						RedSmallWise();
						break;
					case 2:
						RedSmallWise();
						break;
					case 3:
						RedSmallWise();
						break;
					case 4:
						RedThreeFour();
						break;
					case 5:
						RedThreeFour();
						break;
					case 6:
						RedThreeFour();
						break;
				}
				if(correctFlag==0)
				{
					angle-=2;
					if(aimActualPosFlag==3)
					{
						RedThree();
						shootDistance=fabs(sqrt((xShoot-2200)*(xShoot-2200)+(yShoot-41)*(yShoot-41)));
						shootRaps=(-1.651e-6*shootDistance*shootDistance)+0.0204*shootDistance+27.94-1;
						ShooterVelCtrl(shootRaps);
					}
					else if(aimActualPosFlag==4)
					{
						RedFour();
						shootDistance=fabs(sqrt((xShoot+2200)*(xShoot+2200)+(yShoot-41)*(yShoot-41)));
						shootRaps=(-1.949e-6*shootDistance*shootDistance)+0.02135*shootDistance+27.12+3-1;
						ShooterVelCtrl(shootRaps);
					}
					else if(aimActualPosFlag==2)
					{
						RedThree();
						if(mode!=5)
						{
							ShooterVelCtrl(30);
						}						
						else 
						{
							shootDistance=fabs(sqrt((xShoot-2200)*(xShoot-2200)+(yShoot-41)*(yShoot-41)));
							shootRaps=(-1.651e-6*shootDistance*shootDistance)+0.0204*shootDistance+27.94-1;
							ShooterVelCtrl(shootRaps);		
						}
					}
				}
				break;
		}
		if(correctFlag==0)
		{
			if(aimActualPosFlag==1)
			{
				CenterAim();
				shootDistance=fabs(sqrt(xShoot*xShoot+(yShoot-2241)*(yShoot-2241)));
				shootRaps=(-2.249e-6*shootDistance*shootDistance)+0.02486*shootDistance+18.1-0.4;
				ShooterVelCtrl(shootRaps);
			}
		}
	}
	if(keyRedBlueStatus==0)   //选择蓝方
	{
		switch(blueBallStore)
		{
			case 1:  	//蓝方选择1、2号球仓  hong 34
				switch(keyWolkingStatus)
				{
					case 1:
						BlueSmallWise();
						break;
					case 2:
						BlueSmallWise();
						break;
					case 3:
						BlueSmallWise();
						break;
					case 4:
						BlueOneTwo();
						break;
					case 5:
						BlueOneTwo();
						break;
					case 6:
						BlueOneTwo();
						break;
				}
				if(correctFlag==0)
				{
					angle-=2;
					if(aimActualPosFlag==3)
					{
						BlueOne();
						shootDistance=fabs(sqrt((xShoot-2200)*(xShoot-2200)+(yShoot-41)*(yShoot-41)));
						shootRaps=(-1.949e-6*shootDistance*shootDistance)+0.02135*shootDistance+27.12-1;
						ShooterVelCtrl(shootRaps);						
					}
					else if(aimActualPosFlag==4)
					{	
						BlueTwo();
						shootDistance=fabs(sqrt((xShoot+2200)*(xShoot+2200)+(yShoot-41)*(yShoot-41)));
						shootRaps=(-2.828e-6*shootDistance*shootDistance)+0.02413*shootDistance+26.53;
						ShooterVelCtrl(shootRaps);
					}
					else if(aimActualPosFlag==1)
					{
						BlueOne();
						if(mode!=5)
						{
							ShooterVelCtrl(30);
						}						
						else 
						{
							shootDistance=fabs(sqrt((xShoot-2200)*(xShoot-2200)+(yShoot-41)*(yShoot-41)));
							shootRaps=(-1.949e-6*shootDistance*shootDistance)+0.02135*shootDistance+27.12;
							ShooterVelCtrl(shootRaps);	
						}
					}
				}
				break;
			case 2:  	//蓝方选择1、3号球仓   hong 1 3 
				switch(keyWolkingStatus)
				{
					case 1:
						BlueSmallWise();
						break;
					case 2:
						BlueSmallWise();
						break;
					case 3:
						BlueSmallWise();
						break;
					case 4:
//						RedTwoFour();
					BlueTwoFour();
						break;
					case 5:
					BlueTwoFour();
						break;
					case 6:
					BlueTwoFour();
						break;
				}
				if(correctFlag==0)
				{
					angle-=2;
					if(aimActualPosFlag==3||aimActualPosFlag==4)
					{
						if(aimActualPosFlag==3)
						{
							BlueThree();
							shootDistance=fabs(sqrt((xShoot+2200)*(xShoot+2200)+(yShoot-4441)*(yShoot-4441)));
							shootRaps=(-1.651e-6*shootDistance*shootDistance)+0.0204*shootDistance+27.94-1;
							ShooterVelCtrl(shootRaps);
						}
						else
						{
							BlueOne();
							shootDistance=fabs(sqrt((xShoot-2200)*(xShoot-2200)+(yShoot-41)*(yShoot-41)));
							shootRaps=(-1.949e-6*shootDistance*shootDistance)+0.02135*shootDistance+27.12-1;
							ShooterVelCtrl(shootRaps);
						}
					}
					else if(aimActualPosFlag==1)
					{
						BlueOne();
						if(mode!=5)
						{
							ShooterVelCtrl(30);
						}						
						else 
						{
							shootDistance=fabs(sqrt((xShoot-2200)*(xShoot-2200)+(yShoot-41)*(yShoot-41)));
							shootRaps=(-1.949e-6*shootDistance*shootDistance)+0.02135*shootDistance+27.12-1;
							ShooterVelCtrl(shootRaps);	
						}
					}
				}
				break;
			case 3:  	//蓝方选择1、4号球仓  hong2  3
				switch(keyWolkingStatus)
				{
					case 1:
						BlueSmallWise();
						break;
					case 2:
						BlueSmallWise();
						break;
					case 3:
						BlueSmallWise();
						break;
					case 4:
						BlueOneFour();
						break;
					case 5:
						BlueOneFour();
						break;
					case 6:
						BlueOneFour();
						break;
				}
				if(correctFlag==0)
				{
					angle-=2;
					if(aimActualPosFlag==3)
					{
						BlueOne();
						shootDistance=fabs(sqrt((xShoot-2200)*(xShoot-2200)+(yShoot-41)*(yShoot-41)));
						shootRaps=(-1.949e-6*shootDistance*shootDistance)+0.02135*shootDistance+27.12-1;
						ShooterVelCtrl(shootRaps);
					}
					else if(aimActualPosFlag==4)
					{
						BlueFour();
						shootDistance=fabs(sqrt((xShoot-2200)*(xShoot-2200)+(yShoot-4441)*(yShoot-4441)));
						shootRaps=(-1.949e-6*shootDistance*shootDistance)+0.02135*shootDistance+27.12+2;
						ShooterVelCtrl(shootRaps);							
					}
					else if(aimActualPosFlag==1)
					{
						BlueOne();
						if(mode!=5)
						{
							ShooterVelCtrl(30);
						}						
						else 
						{
							shootDistance=fabs(sqrt((xShoot-2200)*(xShoot-2200)+(yShoot-41)*(yShoot-41)));
							shootRaps=(-1.949e-6*shootDistance*shootDistance)+0.02135*shootDistance+27.12-1;
							ShooterVelCtrl(shootRaps);	
						}
					}
				}
				break;
			case 4:  	//蓝方选择2、3号球仓  hong  14
				switch(keyWolkingStatus)
				{
					case 1:
						BlueSmallWise();
						break;
					case 2:
						BlueSmallWise();
						break;
					case 3:
						BlueSmallWise();
						break;
					case 4:
						BlueTwoThree();
						break;
					case 5:
						BlueTwoThree();
						break;
					case 6:
						BlueTwoThree();
						break;
				}
				if(correctFlag==0)
				{
					angle-=2;
					if(aimActualPosFlag==3)
					{
						BlueTwo();
						shootDistance=fabs(sqrt((xShoot+2200)*(xShoot+2200)+(yShoot-41)*(yShoot-41)));
						shootRaps=(-2.828e-6*shootDistance*shootDistance)+0.02413*shootDistance+26.53;
						ShooterVelCtrl(shootRaps);
					}
					else if(aimActualPosFlag==4)
					{
						BlueThree();
						shootDistance=fabs(sqrt((xShoot+2200)*(xShoot+2200)+(yShoot-4441)*(yShoot-4441)));
						shootRaps=(-1.651e-6*shootDistance*shootDistance)+0.0204*shootDistance+27.94-1;
						ShooterVelCtrl(shootRaps);
					}
					else if(aimActualPosFlag==1)
					{
						BlueTwo();
						if(mode!=5)
						{
							ShooterVelCtrl(30);
						}						
						else 
						{
							shootDistance=fabs(sqrt((xShoot+2200)*(xShoot+2200)+(yShoot-41)*(yShoot-41)));
							shootRaps=(-2.828e-6*shootDistance*shootDistance)+0.02413*shootDistance+26.53;
							ShooterVelCtrl(shootRaps);
						}
					}
				}
				break;
			case 5:  	//蓝方选择2、4号球仓
				switch(keyWolkingStatus)
				{
					case 1:
						BlueSmallWise();
						break;
					case 2:
						BlueSmallWise();
						break;
					case 3:
						BlueSmallWise();
						break;
					case 4:
						BlueTwoFour();
						break;
					case 5:
						BlueTwoFour();
						break;
					case 6:
						BlueTwoFour();
						break;
				}
				if(correctFlag==0)
				{
					angle-=2;
					if(aimActualPosFlag==3||aimActualPosFlag==4)
					{
						if(aimActualPosFlag==3)
						{
							BlueFour();
							shootDistance=fabs(sqrt((xShoot-2200)*(xShoot-2200)+(yShoot-4441)*(yShoot-4441)));
							shootRaps=(-1.949e-6*shootDistance*shootDistance)+0.02135*shootDistance+27.12+3-2;
							ShooterVelCtrl(shootRaps);		
						}
						else
						{
							BlueTwo();
							shootDistance=fabs(sqrt((xShoot+2200)*(xShoot+2200)+(yShoot-41)*(yShoot-41)));
							shootRaps=(-2.828e-6*shootDistance*shootDistance)+0.02413*shootDistance+26.53;
							ShooterVelCtrl(shootRaps);
						}
					}
					else if(aimActualPosFlag==1)
					{
						BlueTwo();
						if(mode!=5)
						{
							ShooterVelCtrl(30);
						}						
						else 
						{
							ShooterVelCtrl(30);
	//						shootDistance=fabs(sqrt((xShoot+2200)*(xShoot+2200)+(yShoot-41)*(yShoot-41)));
	//						shootRaps=(-1.949e-6*shootDistance*shootDistance)+0.02135*shootDistance+27.12+2-0.5;
	//						ShooterVelCtrl(shootRaps);
						}
					}
				}
				break;
			case 6:  	//蓝方选择3、4号球仓
				switch(keyWolkingStatus)
				{
					case 1:
						BlueSmallWise();
						break;
					case 2:
						BlueSmallWise();
						break;
					case 3:
						BlueSmallWise();
						break;
					case 4:
						BlueThreeFour();
						break;
					case 5:
						BlueThreeFour();
						break;
					case 6:
						BlueThreeFour();
						break;
				}
				if(correctFlag==0)
				{
					angle-=2;
					if(aimActualPosFlag==3)
					{
						BlueThree();
						shootDistance=fabs(sqrt((xShoot+2200)*(xShoot+2200)+(yShoot-4441)*(yShoot-4441)));
						shootRaps=(-1.651e-6*shootDistance*shootDistance)+0.0204*shootDistance+27.94-1;
						ShooterVelCtrl(shootRaps);
					}
					else if(aimActualPosFlag==4)
					{
						BlueFour();
						shootDistance=fabs(sqrt((xShoot-2200)*(xShoot-2200)+(yShoot-4441)*(yShoot-4441)));
						shootRaps=(-1.949e-6*shootDistance*shootDistance)+0.02135*shootDistance+27.12+3-2;
						ShooterVelCtrl(shootRaps);							
					}
					else if(aimActualPosFlag==1)
					{
						BlueThree();
						if(mode!=5)
						{
							ShooterVelCtrl(30);
						}						
						else 
						{
							shootDistance=fabs(sqrt((xShoot+2200)*(xShoot+2200)+(yShoot-41)*(yShoot-41)));
							shootRaps=(-1.651e-6*shootDistance*shootDistance)+0.0204*shootDistance+27.94-1;
							ShooterVelCtrl(shootRaps);
						}
					}
				}
				break;
		}
		if(correctFlag==0)
		{
			if(aimActualPosFlag==2)     // 黑球
			{
				CenterAim();
				shootDistance=fabs(sqrt(xShoot*xShoot+(yShoot-2241)*(yShoot-2241)));
				shootRaps=(-2.249e-6*shootDistance*shootDistance)+0.02486*shootDistance+18.1-0.4;
				ShooterVelCtrl(shootRaps);
			}
		}
	}
}

/*球仓射击函数*/  
void BlueOneTwo(void)
{
	if(mode!=5)   
	{
		mode5StopAgl=angle;
		Resume();
		collectBallRun();
	}
	if(goBackFlag==0)
	{
		static int bigPosFlag=2,smallPosFlag=2,shootFlag=2;
		switch (mode)
		{
			case 0:
				PidBiggerRoundOri(1.6,800 ,0);
				break;
			case 1:
				PidBiggerRoundOri(1.6,800 ,0);
				break;
			case 2:
				clearEdge(1.4,1);       //扫大边
				if(x>100)
				{
					bigPosFlag=0;
				}
				if(x<-100&&bigPosFlag==0)
				{
					bigPosFlag=1;
				}
				if(bigPosFlag==1&&x>0)
				{
					if(getBallFlag==1)
					{
						mode=5;
						getBallFlag=0;
						correctFlag=1;
					}
					else 
					{
						mode=3;
					}
				}
				break;
		 	case 3:
				clearEdge(0.8,0);				//扫小边
				bigPosFlag=0;
				if(x>100)
				{
					smallPosFlag=0;
				}
				if(x<-100&&smallPosFlag==0)
				{
					smallPosFlag=1;
				}
				if(smallPosFlag==1&&x>0)
				{
					if(getBallFlag==1)
					{
						mode=5;
						getBallFlag=0;
						correctFlag=1;
					}
					else 
					{
						mode=0;
						bigPosFlag=2;
						smallPosFlag=2;
						shootFlag=2;
						detr1=350;
						detv1=100;
						cycleTimes=1;
					}
				}
				break;
			case 4:
				PidBiggerSmallerRoundOri(1.5,1700,0);//投球固定走圆   
				break;
			case 5:
				VelCrl(CAN1, 1,0);
				VelCrl(CAN1, 2,0);
				PidAngle(mode5StopAgl);
				collectBall();
				static int ballNothingCnt=0;
				if((motorActualPos==0&&ballColor[0]==0)||(motorActualPos==1&&ballColor[1]==0))
				{
					ballNothingCnt++;
				}
				if(ballNothingCnt>100)
				{
					if(detr1==350||detr1==0||detr1==700)
					{
						mode=0;
						ballNothingCnt=0;
					}
					else if(detr1==1050&&bigPosFlag==2)
					{
						mode=2;
						ballNothingCnt=0;
					}
					else if(detr1==1050&&bigPosFlag==1)
					{
						mode=3;
						ballNothingCnt=0;
					}
					else
					{
						mode=0;
						bigPosFlag=2;
						smallPosFlag=2;
						shootFlag=2;
						ballNothingCnt=0;
						detr1=350;
						detv1=100;
						cycleTimes=1;
					}
				}
				usartBallNothingCnt=ballNothingCnt;
				usartMotorActualPos=motorActualPos;
				break;
			case 6:
				if(keyWolkingStatus==4)	AnyLineVel(1.6,1,0,-650,1);
				else if(keyWolkingStatus==5)	AnyLineVel(1.6,1,0,-900,1);
				else if(keyWolkingStatus==6)	AnyLineVel(1.6,1,0,-1200,1);
				if(y>2125)
				{
					mode=5;
				}
			break;
		}
		if(mode!=2&&mode!=3)
		{
			if(x<-200)
			{
				shootFlag=1;
			}
			if(x>200)
			{
				shootFlag=0;
			}
			if(x>-100&&shootFlag==1)
			{
				if(getBallFlag==1)
				{
					mode=5;
					getBallFlag=0;
					correctFlag=1;
				}
				shootFlag=0;
			}
		}
	}
}

void BlueOneThree(void)
{
	if(mode!=5)
	{
		mode5StopAgl=angle;
		Resume();
		collectBallRun();
	}
	if(goBackFlag==0)
	{
		static int bigPosFlag=2;
		static int smallPosFlag=2;
		static int shootFlag=2;
		switch (mode)
		{
			case 0:
				PidBiggerRoundOri(1.6,800 ,0);
				break;
			case 1:
				PidBiggerRoundOri(1.6,800 ,0);
				break;
			case 2:
				clearEdge(1.4,1);       //扫大边
				if(x>1000)
				{
					bigPosFlag=0;
				}
				if(x<-1000&&bigPosFlag==0)
				{
					bigPosFlag=1;
				}
				if(bigPosFlag==1&&x>-1000)
				{
					if(getBallFlag==1)
					{
						mode=5;
						getBallFlag=0;
						correctFlag=1;
					}
					else 
					{
						mode=3;
					}
				}
				break;
			case 3:
				clearEdge(0.8,0);				//扫小边
				bigPosFlag=0;
				if(y<2300)
				{
					smallPosFlag=0;
				}
				if(y>2600&&smallPosFlag==0)
				{
					smallPosFlag=1;
				}
				if(smallPosFlag==1&&y<2500)
				{
					if(getBallFlag==1)
					{
						mode=5;
						getBallFlag=0;
						correctFlag=1;
					}
					else 
					{
						mode=0;
						bigPosFlag=2;
						smallPosFlag=2;
						shootFlag=2;
						detr1=350;
						detv1=100;
						cycleTimes=1;
					}
				}
				break;
			case 4:
				PidBiggerSmallerRoundOri(1.5,1700,0);//投球固定走圆   
				break;
			case 5:
				VelCrl(CAN1, 1,0);
				VelCrl(CAN1, 2,0);
				PidAngle(mode5StopAgl);
				collectBall();
				static int ballNothingCnt=0;
				if((motorActualPos==0&&ballColor[0]==0)||(motorActualPos==1&&ballColor[1]==0))
				{
					ballNothingCnt++;
				}
				if(aimActualPosFlag==3&&y<1800) ballNothingCnt=201;
				if(aimActualPosFlag==4&&y>2800) ballNothingCnt=201;
//				if(pinkColorFlag==1&&(motorActualPos==1||motorActualPos==0))
//				{
//					static int pinkTimeCnt=0;
//					if(pinkTimeCnt++>20)
//					{
//						ballNothingCnt=201;
//						pinkColorFlag=0;
//						pinkTimeCnt=0;
//					}
//				}
				if(ballNothingCnt>100)
				{
					if(detr1==350||detr1==0||detr1==700)
					{
						mode=0;
						ballNothingCnt=0;
					}
					if(detr1==1050&&bigPosFlag==2)
					{
						mode=2;
						ballNothingCnt=0;
					}
					if(detr1==1050&&bigPosFlag==1)
					{
						mode=3;
						ballNothingCnt=0;
					}
					if(smallPosFlag==1)
					{
						mode=0;
						bigPosFlag=2;
						smallPosFlag=2;
						shootFlag=0;
						detr1=350;
						detv1=0;
						ballNothingCnt=0;
						cycleTimes=1;
					}
				}
				break;
			case 6:
				if(keyWolkingStatus==4)	AnyLineVel(1.6,1,0,-650,1);
				else if(keyWolkingStatus==5)	AnyLineVel(1.6,1,0,-900,1);
				else if(keyWolkingStatus==6)	AnyLineVel(1.6,1,0,-1200,1);
				if(y>2125)
				{
					mode=5;
				}
			break;
		}
		if(mode!=2&&mode!=3&&detr1!=0)
		{
			if(x>300)
			{
				shootFlag=1;
			}
			if(x<-300)
			{
				shootFlag=0;
			}
			if(x<300&&shootFlag==1)
			{
				if(getBallFlag==1)
				{
					mode=5;
					getBallFlag=0;
					correctFlag=1;
				}
				shootFlag=2;
			}
			if(x>-300&&shootFlag==0)
			{
				if(getBallFlag==1)
				{
					mode=5;
					getBallFlag=0;
					correctFlag=1;
				}
				shootFlag=2;
			}
		}
	}
}
	
void BlueOneFour(void)
{
	if(mode!=5)
	{
		mode5StopAgl=angle;
		Resume();
		collectBallRun();
	}
	if(goBackFlag==0)
	{
		static int bigPosFlag=2;
		static int smallPosFlag=2;
		static int shootFlag=2;   //防止一起步便停下投球，想要如此就改成0
		switch (mode)
		{
			case 0:
				PidBiggerRoundOri(1.6,800 ,0);
				break;
			case 1:
				PidBiggerRoundOri(1.6,800 ,1);
				break;
			case 2:
				clearEdge(1.4,1);       //扫大边
				if(y>2600)
				{
					bigPosFlag=0;
				}
				if(y<2000&&bigPosFlag==0)
				{
					bigPosFlag=1;
				}
				if(bigPosFlag==1&&y>2200)
				{
					if(getBallFlag==1)
					{
						mode=5;
						getBallFlag=0;
						correctFlag=1;
					}
					else 
					{
						mode=3;
					}
				}
				break;
			case 3:
				clearEdge(0.8,0);				//扫小边
				bigPosFlag=0;
				if(y>2600)
				{
					smallPosFlag=0;
				}
				if(y<2000&&smallPosFlag==0)
				{
					smallPosFlag=1;
				}
				if(smallPosFlag==1&&y>2200)
				{
					if(getBallFlag==1)
					{
						mode=5;
						getBallFlag=0;
						correctFlag=1;
					}
					else 
					{
						mode=0;
						bigPosFlag=2;
						smallPosFlag=2;
						shootFlag=2;
						detr1=350;
						detv1=100;
						cycleTimes=1;
					}
				}
				break;
			case 4:
				PidRoundOri(1.6,1400,0);//投球固定走圆 
				break;
			case 5:
				VelCrl(CAN1, 1,0);
				VelCrl(CAN1, 2,0);
				PidAngle(mode5StopAgl);
				collectBall();
				static int ballNothingCnt=0;
				if((motorActualPos==0&&ballColor[0]==0)||(motorActualPos==1&&ballColor[1]==0))
				{
					ballNothingCnt++;
				}
				if(ballNothingCnt>100)
				{
					if(detr1==350||detr1==0||detr1==700)
					{
						mode=0;
						ballNothingCnt=0;
					}
					else if(detr1==1050&&bigPosFlag==2)
					{
						mode=2;
						ballNothingCnt=0;
					}
					else if(detr1==1050&&bigPosFlag==1)
					{
						mode=3;
						ballNothingCnt=0;
					}
					else
					{
						mode=0;
						bigPosFlag=2;
						smallPosFlag=2;
						shootFlag=2;
						ballNothingCnt=0;
						detr1=350;
						detv1=100;
						cycleTimes=1;
					}
				}
				break;
			case 6:
				if(keyWolkingStatus==4)	AnyLineVel(1.6,1,0,-650,1);
				else if(keyWolkingStatus==5)	AnyLineVel(1.6,1,0,-900,1);
				else if(keyWolkingStatus==6)	AnyLineVel(1.6,1,0,-1200,1);
				if(y>2125)
				{
					mode=5;
				}
			break;
		}
		if(mode!=2&&mode!=3)
		{
			if(y>2600)
			{
				shootFlag=0;
			}
			if(y<1800&&shootFlag==0)
			{
				shootFlag=1;
			}
			if(y>2100&&shootFlag==1)
			{
				if(getBallFlag==1)
				{
					mode=5;
					getBallFlag=0;
					correctFlag=1;					
				}
				shootFlag=0;
			}
		}
	}
}

void BlueTwoThree(void)
{
	if(mode!=5)
	{
		mode5StopAgl=angle;
		Resume();
		collectBallRun();
	}
	if(goBackFlag==0)
	{
		static int bigPosFlag=2;
		static int smallPosFlag=2;
		static int shootFlag=2;
		switch (mode)
		{
			case 0:
				PidBiggerRoundOri(1.6,800 ,0);
				break;
			case 1:
				PidBiggerRoundOri(1.6,800 ,0);
				break;
			case 2:
				clearEdge(1.4,1);       //扫大边
				if(y<1200)
				{
					bigPosFlag=0;
				}
				if(y>3000&&bigPosFlag==0)
				{
					bigPosFlag=1;
				}
				if(bigPosFlag==1&&y<2600)
				{
					if(getBallFlag==1)
					{
						mode=5;
						getBallFlag=0;
						correctFlag=1;
					}
					else 
					{
						mode=3;
					}
				}
				break;
			case 3:
				clearEdge(0.8,0);				//扫小边
				bigPosFlag=0;
				if(y<2200)
				{
					smallPosFlag=0;
				}
				if(y>2500&&smallPosFlag==0)
				{
					smallPosFlag=1;
				}
				if(smallPosFlag==1&&y<2400)
				{
					if(getBallFlag==1)
					{
						mode=5;
						getBallFlag=0;
						correctFlag=1;
					}
					else 
					{
						mode=0;
						bigPosFlag=2;
						smallPosFlag=2;
						shootFlag=2;
						detr1=350;
						detv1=100;
						cycleTimes=1;
					}
				}
				break;
			case 4:
				PidRoundOri(1.6,1400,0);//投球固定走圆 
				break;
			case 5:
				VelCrl(CAN1, 1,0);
				VelCrl(CAN1, 2,0);
				PidAngle(mode5StopAgl);
				collectBall();
				static int ballNothingCnt=0;
				if((motorActualPos==0&&ballColor[0]==0)||(motorActualPos==1&&ballColor[1]==0))
				{
					ballNothingCnt++;
				}
				if(ballNothingCnt>100)
				{
					if(detr1==350||detr1==0||detr1==700)
					{
						mode=0;
						ballNothingCnt=0;
					}
					else if(detr1==1050&&bigPosFlag==2)
					{
						mode=2;
						ballNothingCnt=0;
					}
					else if(detr1==1050&&bigPosFlag==1)
					{
						mode=3;
						ballNothingCnt=0;
					}
					else
					{
						mode=0;
						bigPosFlag=2;
						smallPosFlag=2;
						shootFlag=2;
						ballNothingCnt=0;
						detr1=350;
						detv1=100;
						cycleTimes=1;
					}					
				}
				break;
			case 6:
				if(keyWolkingStatus==4)	AnyLineVel(1.6,1,0,-650,1);
				else if(keyWolkingStatus==5)	AnyLineVel(1.6,1,0,-900,1);
				else if(keyWolkingStatus==6)	AnyLineVel(1.6,1,0,-1200,1);
				if(y>2125)
				{
					mode=5;
				}
			break;
		}
		if(mode!=2&&mode!=3)
		{
			if(y>3000)
			{
				shootFlag=1;
			}
			if(y<2000)
			{
				shootFlag=0;
			}
			if(y<2600&&shootFlag==1)
			{
				if(getBallFlag==1)
				{
					mode=5;	
					getBallFlag=0;
					correctFlag=1;
				}
				shootFlag=0;
			}
		}
	}
}

void BlueTwoFour(void)
{
	if(mode!=5)
	{
		mode5StopAgl=angle;
		Resume();
		collectBallRun();
	}
	if(goBackFlag==0)
	{
		static int bigPosFlag=2,smallPosFlag=2,shootFlag=2;	
		switch (mode)
		{
			case 0:
				PidBiggerRoundOri(1.6,800 ,0);
				break;
			case 1:
				PidBiggerRoundOri(1.6,800 ,0);
				break;
			case 2:
				clearEdge(1.4,1);       //扫大边
				if(x>1000)
				{
					bigPosFlag=0;
				}
				if(x<-1000&&bigPosFlag==0)
				{
					bigPosFlag=1;
				}
				if(bigPosFlag==1&&x>-1000)
				{
					if(getBallFlag==1)
					{
						mode=5;
						getBallFlag=0;
						correctFlag=1;
					}
					else 
					{
						mode=3;
					}
				}
				break;
			case 3:
				clearEdge(0.8,0);				//扫小边
				bigPosFlag=0;
				if(y<2300)
				{
					smallPosFlag=0;
				}
				if(y>2600&&smallPosFlag==0)
				{
					smallPosFlag=1;
				}
				if(smallPosFlag==1&&y<2500)
				{
					if(getBallFlag==1)
					{
						mode=5;
						getBallFlag=0;
						correctFlag=1;
					}
					else 
					{
						mode=0;
						bigPosFlag=2;
						smallPosFlag=2;
						shootFlag=2;
						detr1=350;
						detv1=100;
						cycleTimes=1;
					}
				}
				break;
			case 4:
				PidBiggerSmallerRoundOri(1.5,1700,0);//投球固定走圆   
				break;
			case 5:
				VelCrl(CAN1, 1,0);
				VelCrl(CAN1, 2,0);
				PidAngle(mode5StopAgl);
				collectBall();
				static int ballNothingCnt=0;
				if((motorActualPos==0&&ballColor[0]==0)||(motorActualPos==1&&ballColor[1]==0))
				{
					ballNothingCnt++;
				}
				if(keyRedBlueStatus==0)    //blue
				{
					if(aimActualPosFlag==3&&y<1600)
					{
						ballNothingCnt=201;
	//					colorChangeFlag=!colorChangeFlag;
						stopCollectRunFlag=1;
					}
					if(aimActualPosFlag==4&&y>3200)
					{
						ballNothingCnt=201;
	//					colorChangeFlag=!colorChangeFlag;
						stopCollectRunFlag=1;
					}
				}
				else if(keyRedBlueStatus==1)  //hong
				{
					if(aimActualPosFlag==3&&y<1600)
					{
						ballNothingCnt=201;
	//					colorChangeFlag=!colorChangeFlag;
						stopCollectRunFlag=1;
					}
					if(aimActualPosFlag==4&&y>3000)
					{
						ballNothingCnt=201;
	//					colorChangeFlag=!colorChangeFlag;
						stopCollectRunFlag=1;
					}
				}
				if(ballNothingCnt>100&&lastMode!=6)
				{
					if(detr1==350||detr1==0||detr1==700)
					{
						mode=0;
						ballNothingCnt=0;
					}
					if(detr1==1050&&bigPosFlag==2)
					{
						mode=2;
						ballNothingCnt=0;
					}
					if(detr1==1050&&bigPosFlag==1)
					{
						mode=3;
						ballNothingCnt=0;
					}
					if(smallPosFlag==1)
					{
						mode=0;
						bigPosFlag=2;
						smallPosFlag=2;
						shootFlag=0;
						detr1=350;
						detv1=0;
						ballNothingCnt=0;
						cycleTimes=1;
					}
				}
				if(ballNothingCnt>200)
				{
					if(detr1==350||detr1==0||detr1==700||detr1==-350)
					{
						mode=0;
						ballNothingCnt=0;
					}
					if(detr1==1050&&bigPosFlag==2)
					{
						mode=2;
						ballNothingCnt=0;
					}
					if(detr1==1050&&bigPosFlag==1)
					{
						mode=3;
						ballNothingCnt=0;
					}
					if(smallPosFlag==1)
					{
						mode=0;
						bigPosFlag=2;
						smallPosFlag=2;
						shootFlag=0;
						detr1=350;
						detv1=0;
						ballNothingCnt=0;
						cycleTimes=1;
					}
				}
				break;
			case 6:
				if(keyWolkingStatus==4)	AnyLineVel(1.6,1,0,-650,1);
				else if(keyWolkingStatus==5)	AnyLineVel(1.6,1,0,-900,1);
				else if(keyWolkingStatus==6)	AnyLineVel(1.6,1,0,-1200,1);
				if(y>2225)
				{
					mode=5;
				}
			break;
		}
		if(mode!=2&&mode!=3&&detr1!=0)
		{
			if(x>300)
			{
				shootFlag=1;
			}
			if(x<-300)
			{
				shootFlag=0;
			}
			if(x<0&&shootFlag==1)
			{
				if(getBallFlag==1)
				{
					mode=5;
					getBallFlag=0;
					correctFlag=1;
				}
				shootFlag=2;
			}
			if(x>0&&shootFlag==0)
			{
				if(getBallFlag==1)
				{
					mode=5;
					getBallFlag=0;
					correctFlag=1;
				}
				shootFlag=2;
			}
		}
	}
}

void BlueThreeFour(void)
{
	if(mode!=5)
	{
		mode5StopAgl=angle;
		Resume();
		collectBallRun();
	}
	if(goBackFlag==0)
	{
		static int bigPosFlag=2;
		static int smallPosFlag=2;
		static int shootFlag=2;
		switch (mode)
		{
			case 0:
				PidBiggerRoundOri(1.6,800 ,0);
				break;
			case 1:
				PidBiggerRoundOri(1.6,800 ,0);
				break;
			case 2:
				clearEdge(1.4,1);       //扫大边
				if(x<-100)
				{
					bigPosFlag=0;
				}
				if(x>100&&bigPosFlag==0)
				{
					bigPosFlag=1;
				}
				if(bigPosFlag==1&&x<0)
				{
					if(getBallFlag==1)
					{
						mode=5;
						getBallFlag=0;
						correctFlag=1;
					}
					else 
					{
						mode=3;
					}
				}
				break;
			case 3:
				clearEdge(0.8,0);				//扫小边
				bigPosFlag=0;
				if(x<-100)
				{
					smallPosFlag=0;
				}
				if(x>100&&smallPosFlag==0)
				{
					smallPosFlag=1;
				}
				if(smallPosFlag==1&&x<0)
				{
					if(getBallFlag==1)
					{
						mode=5;
						getBallFlag=0;
						correctFlag=1;
					}
					else 
					{
						mode=0;
						bigPosFlag=2;
						smallPosFlag=2;
						shootFlag=2;
						detr1=350;
						detv1=100;
						cycleTimes=1;
					}
				}
				break;
			case 4:
				PidRoundOri(1.6,1400,0);//投球固定走圆 
				break;
			case 5:
				VelCrl(CAN1, 1,0);
				VelCrl(CAN1, 2,0);
				PidAngle(mode5StopAgl);
				collectBall();
				static int ballNothingCnt=0;
				if((motorActualPos==0&&ballColor[0]==0)||(motorActualPos==1&&ballColor[1]==0))
				{
					ballNothingCnt++;
				}
				if(ballNothingCnt>100)
				{
					if(detr1==350||detr1==0)
					{
						mode=0;
						ballNothingCnt=0;
					}
					else if(detr1==700)
					{
						mode=0;
						ballNothingCnt=0;
					}
					else if(detr1==1050&&bigPosFlag==2)
					{
						mode=2;
						ballNothingCnt=0;
					}
					else if(detr1==1050&&bigPosFlag==1)
					{
						mode=3;
						ballNothingCnt=0;
					}
					else
					{
						mode=0;
						bigPosFlag=2;
						smallPosFlag=2;
						shootFlag=2;
						ballNothingCnt=0;
						detr1=350;
						detv1=100;
						cycleTimes=1;
					}
				}
				break;
			case 6:
				if(keyWolkingStatus==4)	AnyLineVel(1.6,1,0,-650,1);
				else if(keyWolkingStatus==5)	AnyLineVel(1.6,1,0,-900,1);
				else if(keyWolkingStatus==6)	AnyLineVel(1.6,1,0,-1200,1);
				if(y>2125)
				{
					mode=5;
				}
			break;
		}
		if(mode!=2&&mode!=3)
		{
			if(x>200)
			{
				shootFlag=1;
			}
			if(x<-200)
			{
				shootFlag=0;
			}
			if(x<100&&shootFlag==1)
			{
				if(getBallFlag==1)
				{
					mode=5;		
					getBallFlag=0;
					correctFlag=1;					
				}
				shootFlag=0;
			}
		}
	}
}

void RedOneTwo(void)
{
	if(mode!=5)
	{
		mode5StopAgl=angle;
		Resume();
		collectBallRun();
	}
	if(goBackFlag==0)
	{
		static int bigPosFlag=2;
		static int smallPosFlag=2;
		static int shootFlag=2;
		switch (mode)
		{
			case 0:
				PidBiggerRoundOri(1.6,700 ,0);
				break;
			case 1:
				PidBiggerRoundOri(1.6,700 ,0);
				break;
			case 2:
				clearEdge(1.4,1);       //扫大边
				if(x<-100)
				{
					bigPosFlag=0;
				}
				if(x>100&&bigPosFlag==0)
				{
					bigPosFlag=1;
				}
				if(bigPosFlag==1&&x<0)
				{
					if(getBallFlag==1)
					{
						mode=5;
						getBallFlag=0;
						correctFlag=1;
					}
					else 
					{
						mode=3;
					}
				}
				break;
			case 3:
				clearEdge(0.8,0);				//扫小边
				bigPosFlag=0;
				if(x<-100)
				{
					smallPosFlag=0;
				}
				if(x>100&&smallPosFlag==0)
				{
					smallPosFlag=1;
				}
				if(smallPosFlag==1&&x<0)
				{
					if(getBallFlag==1)
					{
						mode=5;
						getBallFlag=0;
						correctFlag=1;
					}
					else 
					{
						mode=0;
						bigPosFlag=2;
						smallPosFlag=2;
						shootFlag=2;
						detr1=350;
						detv1=100;
						cycleTimes=1;
					}
				}
				break;
			case 4:
				PidBiggerSmallerRoundOri(1.5,1700,0);//投球固定走圆 
				break;
			case 5:
				VelCrl(CAN1, 1,0);
				VelCrl(CAN1, 2,0);
				PidAngle(mode5StopAgl);
				collectBall();
				static int ballNothingCnt=0;
				if((motorActualPos==0&&ballColor[0]==0)||(motorActualPos==1&&ballColor[1]==0))
				{
					ballNothingCnt++;
				}
				if(ballNothingCnt>100)
				{
					if(detr1==350||detr1==0||detr1==700)
					{
						mode=0;
						ballNothingCnt=0;
					}
					else if(detr1==1050&&bigPosFlag==2)
					{
						mode=2;
						ballNothingCnt=0;
					}
					else if(detr1==1050&&bigPosFlag==1)
					{
						mode=3;
						ballNothingCnt=0;
					}
					else
					{
						mode=0;
						bigPosFlag=2;
						smallPosFlag=2;
						shootFlag=2;
						ballNothingCnt=0;
						detr1=350;
						detv1=100;
						cycleTimes=1;
					}
				}
				break;
			case 6:
				if(keyWolkingStatus==4)	AnyLineVel(1.6,1,0,-650,1);
				else if(keyWolkingStatus==5)	AnyLineVel(1.6,1,0,-900,1);
				else if(keyWolkingStatus==6)	AnyLineVel(1.6,1,0,-1200,1);
				if(y>2125)
				{
					mode=5;
				}
			break;
		}
		if(mode!=2&&mode!=3)
		{
			if(x>400)
			{
				shootFlag=1;
			}
			if(x<-300)
			{
				shootFlag=0;
			}
			if(x<200&&shootFlag==1)
			{
				if(getBallFlag==1)
				{
					mode=5;
					getBallFlag=0;
					correctFlag=1;
				}
				shootFlag=0;
			}
		}
	}
}
void RedOneThree(void)
{
	if(mode!=5)
	{
		mode5StopAgl=angle;
		Resume();
		collectBallRun();
	}
	if(goBackFlag==0)
	{
		static int bigPosFlag=2,smallPosFlag=2,shootFlag=2;
		switch (mode)
		{
			case 0:
				PidBiggerRoundOri(1.6,800 ,0);
				break;
			case 1:
				PidBiggerRoundOri(1.6,800 ,1);
				break;
			case 2:
				clearEdge(1.4,1);       //扫大边
				if(x>1000)
				{
					bigPosFlag=0;
				}
				if(x<-1000&&bigPosFlag==0)
				{
					bigPosFlag=1;
				}
				if(bigPosFlag==1&&x>-1000)
				{
					if(getBallFlag==1)
					{
						mode=5;
						getBallFlag=0;
						correctFlag=1;
					}
					else 
					{
						mode=3;
					}
				}
				break;
			case 3:
				clearEdge(0.8,0);				//扫小边
				bigPosFlag=0;
				if(y<2300)
				{
					smallPosFlag=0;
				}
				if(y>2600&&smallPosFlag==0)
				{
					smallPosFlag=1;
				}
				if(smallPosFlag==1&&y<2500)
				{
					if(getBallFlag==1)
					{
						mode=5;
						getBallFlag=0;
						correctFlag=1;
					}
					else 
					{
						mode=0;
						bigPosFlag=2;
						smallPosFlag=2;
						shootFlag=2;
						detr1=350;
						detv1=100;
						cycleTimes=1;
					}
				}
				break;
			case 4:
				PidBiggerSmallerRoundOri(1.5,1700,0);//投球固定走圆   
				break;
			case 5:
				VelCrl(CAN1, 1,0);
				VelCrl(CAN1, 2,0);
				PidAngle(mode5StopAgl);
				collectBall();
				static int ballNothingCnt=0;
				if((motorActualPos==0&&ballColor[0]==0)||(motorActualPos==1&&ballColor[1]==0))
				{
					ballNothingCnt++;
				}
				if(pinkColorFlag==1&&(motorActualPos==1||motorActualPos==0))
				{
					ballNothingCnt=201;
					pinkColorFlag=0;
				}
				if(ballNothingCnt>100)
				{
					if(detr1==350||detr1==0||detr1==700)
					{
						mode=0;
						ballNothingCnt=0;
					}
					if(detr1==1050&&bigPosFlag==2)
					{
						mode=2;
						ballNothingCnt=0;
					}
					if(detr1==1050&&bigPosFlag==1)
					{
						mode=3;
						ballNothingCnt=0;
					}
					if(smallPosFlag==1)
					{
						mode=0;
						bigPosFlag=2;
						smallPosFlag=2;
						shootFlag=0;
						detr1=350;
						detv1=0;
						ballNothingCnt=0;
						cycleTimes=1;
					}
				}
				break;
			case 6:
				if(keyWolkingStatus==4)	AnyLineVel(1.6,1,0,-650,1);
				else if(keyWolkingStatus==5)	AnyLineVel(1.6,1,0,-900,1);
				else if(keyWolkingStatus==6)	AnyLineVel(1.6,1,0,-1200,1);
				if(y>2125)
				{
					mode=5;
				}
			break;
		}
		if(mode!=2&&mode!=3&&detr1!=0)
		{
			if(x>300)
			{
				shootFlag=1;
			}
			if(x<-300)
			{
				shootFlag=0;
			}
			if(x<0&&shootFlag==1)
			{
				if(getBallFlag==1)
				{
					mode=5;
					getBallFlag=0;
					correctFlag=1;
				}
				shootFlag=2;
			}
			if(x>-0&&shootFlag==0)
			{
				if(getBallFlag==1)
				{
					mode=5;
					getBallFlag=0;
					correctFlag=1;
				}
				shootFlag=2;
			}
		}
	}
}
void RedOneFour(void)
{
	if(mode!=5)
	{
		mode5StopAgl=angle;
		Resume();
		collectBallRun();
	}
	if(goBackFlag==0)
	{
		static int bigPosFlag=2;
		static int smallPosFlag=2;
		static int shootFlag=0;
		switch (mode)
		{
			case 0:
				PidBiggerRoundOri(1.6,800 ,0);
				break;
			case 1:
				PidBiggerRoundOri(1.6,800 ,0);
				break;
			case 2:
				clearEdge(1.4,1);       //扫大边
				if(y<1200)
				{
					bigPosFlag=0;
				}
				if(y>3000&&bigPosFlag==0)
				{
					bigPosFlag=1;
				}
				if(bigPosFlag==1&&y<2600)
				{
					if(getBallFlag==1)
					{
						mode=5;
						getBallFlag=0;
						correctFlag=1;
					}
					else 
					{
						mode=3;
					}
				}
				break;
			case 3:
				clearEdge(0.8,0);				//扫小边
				bigPosFlag=0;
				if(y<2200)
				{
					smallPosFlag=0;
				}
				if(y>2600&&smallPosFlag==0)
				{
					smallPosFlag=1;
				}
				if(smallPosFlag==1&&y<2400)
				{
					if(getBallFlag==1)
					{
						mode=5;
						getBallFlag=0;
						correctFlag=1;
					}
					else 
					{
						mode=0;
						bigPosFlag=2;
						smallPosFlag=2;
						shootFlag=2;
						detr1=350;
						detv1=100;
						cycleTimes=1;
					}
				}
				break;
			case 4:
				PidRoundOri(1.6,1400,0);//投球固定走圆 
				break;
			case 5:
				VelCrl(CAN1, 1,0);
				VelCrl(CAN1, 2,0);
				PidAngle(mode5StopAgl);
				collectBall();
				static int ballNothingCnt=0;
				if((motorActualPos==0&&ballColor[0]==0)||(motorActualPos==1&&ballColor[1]==0))
				{
					ballNothingCnt++;
				}
				if(ballNothingCnt>100)
				{
					if(detr1==350||detr1==0||detr1==700)
					{
						mode=0;
						ballNothingCnt=0;
					}
					else if(detr1==1050&&bigPosFlag==2)
					{
						mode=2;
						ballNothingCnt=0;
					}
					else if(detr1==1050&&bigPosFlag==1)
					{
						mode=3;
						ballNothingCnt=0;
					}
					else
					{
						mode=0;
						bigPosFlag=2;
						smallPosFlag=2;
						shootFlag=2;
						ballNothingCnt=0;
						detr1=350;
						detv1=100;
						cycleTimes=1;
					}					
				}
				break;
			case 6:
				if(keyWolkingStatus==4)	AnyLineVel(1.6,1,0,-650,1);
				else if(keyWolkingStatus==5)	AnyLineVel(1.6,1,0,-900,1);
				else if(keyWolkingStatus==6)	AnyLineVel(1.6,1,0,-1200,1);
				if(y>2125)
				{
					mode=5;
				}
			break;
		}
		if(mode!=2&&mode!=3)
		{
			if(y>2800)
			{
				shootFlag=1;
			}
			if(y<2000)
			{
				shootFlag=0;
			}
			if(y<2500&&shootFlag==1)
			{
				if(getBallFlag==1)
				{
					mode=5;	
					getBallFlag=0;	
					correctFlag=1;
				}
				shootFlag=0;
			}
		}
	}
}
void RedTwoThree(void)
{
	if(mode!=5)
	{
		mode5StopAgl=angle;
		Resume();
		collectBallRun();
	}
	if(goBackFlag==0)
	{
		static int bigPosFlag=2;
		static int smallPosFlag=2;
		static int shootFlag=2;
		switch (mode)
		{
			case 0:
				PidBiggerRoundOri(1.6,800 ,0);
				break;
			case 1:
				PidBiggerRoundOri(1.6,800 ,1);
				break;
			case 2:
				clearEdge(1.4,1);       //扫大边
				if(y>2800)
				{
					bigPosFlag=0;
				}
				if(y<2000&&bigPosFlag==0)
				{
					bigPosFlag=1;
				}
				if(bigPosFlag==1&&y>2200)
				{
					if(getBallFlag==1)
					{
						mode=5;
						getBallFlag=0;
						correctFlag=1;
					}
					else 
					{
						mode=3;
					}
				}
				break;
			case 3:
				clearEdge(0.8,0);				//扫小边
				bigPosFlag=0;
				if(y>2600)
				{
					smallPosFlag=0;
				}
				if(y<2200&&smallPosFlag==0)
				{
					smallPosFlag=1;
				}
				if(smallPosFlag==1&&y>2400)
				{
					if(getBallFlag==1)
					{
						mode=5;
						getBallFlag=0;
						correctFlag=1;
					}
					else 
					{
						mode=0;
						bigPosFlag=2;
						smallPosFlag=2;
						shootFlag=2;
						detr1=350;
						detv1=100;
						cycleTimes=1;
					}
				}
				break;
			case 4:
				PidRoundOri(1.6,1400,0);//投球固定走圆 
				break;
			case 5:
				VelCrl(CAN1, 1,0);
				VelCrl(CAN1, 2,0);
				PidAngle(mode5StopAgl);
				collectBall();
				static int ballNothingCnt=0;
				if((motorActualPos==0&&ballColor[0]==0)||(motorActualPos==1&&ballColor[1]==0))
				{
					ballNothingCnt++;
				}
				if(ballNothingCnt>100)
				{
					if(detr1==350||detr1==0)
					{
						mode=0;
						ballNothingCnt=0;
					}
					else if(detr1==700)
					{
						mode=0;
						ballNothingCnt=0;
					}
					else if(detr1==1050&&bigPosFlag==2)
					{
						mode=2;
						ballNothingCnt=0;
					}
					else if(detr1==1050&&bigPosFlag==1)
					{
						mode=3;
						ballNothingCnt=0;
					}
					else
					{
						mode=0;
						bigPosFlag=2;
						smallPosFlag=2;
						shootFlag=2;
						ballNothingCnt=0;
						detr1=350;
						detv1=100;
						cycleTimes=1;
					}
				}
				break;
			case 6:
				if(keyWolkingStatus==4)	AnyLineVel(1.6,1,0,-650,1);
				else if(keyWolkingStatus==5)	AnyLineVel(1.6,1,0,-900,1);
				else if(keyWolkingStatus==6)	AnyLineVel(1.6,1,0,-1200,1);
				if(y>2125)
				{
					mode=5;
				}
			break;
		}
		if(mode!=2&&mode!=3)
		{
			if(y>2600)
			{
				shootFlag=0;
			}
			if(y<1800&&shootFlag==0)
			{
				shootFlag=1;
			}
			if(y>2200&&shootFlag==1)
			{
				if(getBallFlag==1)
				{
					mode=5;
					getBallFlag=0;	
					correctFlag=1;
				}
				shootFlag=0;
			}
		}
	}
}
void RedTwoFour(void)
{
	if(mode!=5)
	{
		mode5StopAgl=angle;
		Resume();
		collectBallRun();
	}
	if(goBackFlag==0)
	{
		static int bigPosFlag=2;
		static int smallPosFlag=2;
		static int shootFlag=2;
		switch (mode)
		{
			case 0:
				PidBiggerRoundOri(1.6,800 ,0);
				break;
			case 1:
				PidBiggerRoundOri(1.6,800 ,0);
				break;
			case 2:
				clearEdge(1.4,1);       //扫大边
				if(x>1000)
				{
					bigPosFlag=0;
				}
				if(x<-1000&&bigPosFlag==0)
				{
					bigPosFlag=1;
				}
				if(bigPosFlag==1&&x>-1000)
				{
					if(getBallFlag==1)
					{
						mode=5;
						getBallFlag=0;
						correctFlag=1;
					}
					else 
					{
						mode=3;
					}
				}
				break;
			case 3:
				clearEdge(0.8,0);				//扫小边
				bigPosFlag=0;
				if(y<2300)
				{
					smallPosFlag=0;
				}
				if(y>2600&&smallPosFlag==0)
				{
					smallPosFlag=1;
				}
				if(smallPosFlag==1&&y<2500)
				{
					if(getBallFlag==1)
					{
						mode=5;
						getBallFlag=0;
						correctFlag=1;
					}
					else 
					{
						mode=0;
						bigPosFlag=2;
						smallPosFlag=2;
						shootFlag=2;
						detr1=350;
						detv1=100;
						cycleTimes=1;
					}
				}
				break;
			case 4:
				PidBiggerSmallerRoundOri(1.5,1700,0);//投球固定走圆   
				break;
			case 5:
				VelCrl(CAN1, 1,0);
				VelCrl(CAN1, 2,0);
				PidAngle(mode5StopAgl);
				collectBall();
				static int ballNothingCnt=0;
				if((motorActualPos==0&&ballColor[0]==0)||(motorActualPos==1&&ballColor[1]==0))
				{
					ballNothingCnt++;
				}
				if(pinkColorFlag==1&&(motorActualPos==1||motorActualPos==0))
				{
					ballNothingCnt=201;
					pinkColorFlag=0;
				}
				if(ballNothingCnt>100)
				{
					if(detr1==350||detr1==0)
					{
						mode=0;
						ballNothingCnt=0;
					}
					if(detr1==700)
					{
						mode=0;
						ballNothingCnt=0;
					}
					if(detr1==1050&&bigPosFlag==2)
					{
						mode=2;
						ballNothingCnt=0;
					}
					if(detr1==1050&&bigPosFlag==1)
					{
						mode=3;
						ballNothingCnt=0;
					}
					if(smallPosFlag==1)
					{
						mode=0;
						bigPosFlag=2;
						smallPosFlag=2;
						shootFlag=0;
						detr1=350;
						detv1=0;
						ballNothingCnt=0;
						cycleTimes=1;
					}
				}
				break;
			case 6:
				if(keyWolkingStatus==4)	AnyLineVel(1.6,1,0,-650,1);
				else if(keyWolkingStatus==5)	AnyLineVel(1.6,1,0,-900,1);
				else if(keyWolkingStatus==6)	AnyLineVel(1.6,1,0,-1200,1);
				if(y>2125)
				{
					mode=5;
				}
			break;
		}
		if(mode!=2&&mode!=3&&detr1!=0)
		{
			if(x>300)
			{
				shootFlag=1;
			}
			if(x<-300)
			{
				shootFlag=0;
			}
			if(x<0&&shootFlag==1)
			{
				if(getBallFlag==1)
				{
					mode=5;
					getBallFlag=0;
					correctFlag=1;
				}
				shootFlag=2;
			}
			if(x>0&&shootFlag==0)
			{
				if(getBallFlag==1)
				{
					mode=5;
					getBallFlag=0;
					correctFlag=1;
				}
				shootFlag=2;
			}
		}
	}
}
void RedThreeFour(void)
{
	if(mode!=5)
	{
		mode5StopAgl=angle;
		Resume();
		collectBallRun();
	}
	if(goBackFlag==0)
	{
		static int bigPosFlag=2;
		static int smallPosFlag=2;
		static int shootFlag=2;
		switch (mode)
		{
			case 0:
				PidBiggerRoundOri(1.6,800 ,0);
				break;
			case 1:
				PidBiggerRoundOri(1.6,800 ,0);
				break;
			case 2:
				clearEdge(1.4,1);       //扫大边
				if(x>100)
				{
					bigPosFlag=0;
				}
				if(x<-100&&bigPosFlag==0)
				{
					bigPosFlag=1;
				}
				if(bigPosFlag==1&&x>0)
				{
					if(getBallFlag==1)
					{
						mode=5;
						getBallFlag=0;
						correctFlag=1;
					}
					else 
					{
						mode=3;
					}
				}
				break;
			case 3:
				clearEdge(0.8,0);				//扫小边
				bigPosFlag=0;
				if(x>100)
				{
					smallPosFlag=0;
				}
				if(x<100&&smallPosFlag==0)
				{
					smallPosFlag=1;
				}
				if(smallPosFlag==1&&x>0)
				{
					if(getBallFlag==1)
					{
						mode=5;
						getBallFlag=0;
						correctFlag=1;
					}
					else 
					{
						mode=0;
						bigPosFlag=2;
						smallPosFlag=2;
						shootFlag=2;
						detr1=350;
						detv1=100;
						cycleTimes=1;
					}
				}
				break;
			case 4:
				PidRoundOri(1.6,1400,0);//投球固定走圆 
				break;
			case 5:
				VelCrl(CAN1, 1,0);
				VelCrl(CAN1, 2,0);
				PidAngle(mode5StopAgl);
				collectBall();
				static int ballNothingCnt=0;
				if((motorActualPos==0&&ballColor[0]==0)||(motorActualPos==1&&ballColor[1]==0))
				{
					ballNothingCnt++;
				}
				if(ballNothingCnt>100)
				{
					if(detr1==350||detr1==0||detr1==700)
					{
						mode=0;
						ballNothingCnt=0;
					}
					else if(detr1==1050&&bigPosFlag==2)
					{
						mode=2;
						ballNothingCnt=0;
					}
					else if(detr1==1050&&bigPosFlag==1)
					{
						mode=3;
						ballNothingCnt=0;
					}
					else
					{
						mode=0;
						bigPosFlag=2;
						smallPosFlag=2;
						shootFlag=2;
						detr1=350;
						detv1=100;
						cycleTimes=1;
					}
				}
				break;
			case 6:
				if(keyWolkingStatus==4)	AnyLineVel(1.6,1,0,-650,1);
				else if(keyWolkingStatus==5)	AnyLineVel(1.6,1,0,-900,1);
				else if(keyWolkingStatus==6)	AnyLineVel(1.6,1,0,-1200,1);
				if(y>2125)
				{
					mode=5;
				}
			break;
		}
		if(mode!=2&&mode!=3)
		{
			if(x<-50)
			{
				shootFlag=1;
			}
			if(x>50)
			{
				shootFlag=0;
			}
			if(x>-20&&shootFlag==1)
			{
				if(getBallFlag==1)
				{
					mode=5;
					getBallFlag=0;
					correctFlag=1;
				}
				shootFlag=0;
			}
		}
	}
}
/* Private typedef ------------------------------------------------------------------------------------*/
/* Private define -------------------------------------------------------------------------------------*/
/* Private macro --------------------------------------------------------------------------------------*/
/* Private variables ----------------------------------------------------------------------------------*/
/* Private function prototypes ------------------------------------------------------------------------*/
/* Private functions ----------------------------------------------------------------------------------*/

/********************* (C) COPYRIGHT NEU_ACTION_2018 ****************END OF FILE************************/
